Skip to content

Commit

Permalink
[Ninja][Assembly Motion] Workaround to use trajectory tracking in ass…
Browse files Browse the repository at this point in the history 
…embly motion.
  • Loading branch information
@sugihara-16
sugihara-16 committed Nov 21, 2024
1 parent 26597b9 commit 2b1e5c6
Show file tree
Hide file tree
Showing 3 changed files with 58 additions and 53 deletions.
2 changes: 2 additions & 0 deletions robots/ninja/launch/assembly_tasks/assembly_motion.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,10 @@
<launch>
<arg name = "module_ids" default=""/>
<arg name = "real_machine" default="true"/>
<arg name = "approach_mode" default="nav"/>
<node pkg="ninja" type="assembly_motion.py" name="" output="screen" >
<param name = "module_ids" value="$(arg module_ids)"/>
<param name = "real_machine" value="$(arg real_machine)"/>
<param name = "approach_mode" value="$(arg approach_mode)"/>
</node>
</launch>
97 changes: 48 additions & 49 deletions robots/ninja/script/ninja/assembly_api.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,8 @@ def __init__(self,
self.standby_servo_angle_male = standby_servo_angle_male
self.leader = leader
self.leader_id = leader_id
self.leader_target_pose = []
self.leader_pre_target_pose = []
self.target_offset = np.zeros(4)
self.x_offset = x_offset
self.y_offset = y_offset
Expand All @@ -104,7 +106,6 @@ def __init__(self,
# flags
self.emergency_flag = False
self.follower_male_mech_activated = False
self.leader_target_pose_set_flag = False

# tf listener and broadcaster
self.listener = tf.TransformListener()
Expand Down Expand Up @@ -140,9 +141,6 @@ def __init__(self,
self.att_error_tol = np.array([self.roll_tol, self.pitch_tol, self.yaw_tol]) # attitude error torelance

def execute(self, userdata):
if not self.leader_target_pose_set_flag:
return 'in_process'

if not self.follower_male_mech_activated:
if self.real_machine:
self.dynamixel_servo.sendTargetAngle(self.standby_servo_angle_male)
Expand Down Expand Up @@ -184,7 +182,6 @@ def execute(self, userdata):
elif self.emergency_flag:
return 'emergency'
else:
# Nav(x,y,z and yaw)
nav_msg_follower = FlightNav()
nav_msg_follower.target = 1
nav_msg_follower.control_frame = 0
Expand All @@ -195,25 +192,27 @@ def execute(self, userdata):
nav_msg_follower.target_pos_y = self.target_cog_pos[1]
nav_msg_follower.target_pos_z = self.target_cog_pos[2]
nav_msg_follower.target_yaw = self.target_att[2]
# Trajectory(x,y,z and yaw)
# traj_msg_follower = PoseStamped()
# target_pos_cog = self.coordTransformer.posTransform('root','cog',target_pos)
# traj_msg_follower.header.stamp = rospy.Time.now()
# traj_msg_follower.pose.position.x = target_pos_cog[0]
# traj_msg_follower.pose.position.y = target_pos_cog[1]
# traj_msg_follower.pose.position.z = target_pos_cog[2]
# traj_msg_follower.pose.orientation.x = homo_transformed_target_odom[1][0]
# traj_msg_follower.pose.orientation.y = homo_transformed_target_odom[1][1]
# traj_msg_follower.pose.orientation.z = homo_transformed_target_odom[1][2]
# traj_msg_follower.pose.orientation.w = homo_transformed_target_odom[1][3]

if(self.approach_mode == 'nav'):
self.follower_nav_pub.publish(nav_msg_follower)
elif(self.approach_mode == 'trajectory'):
self.follower_traj_pub.publish(traj_msg_follower)
else:
rospy.logerr("Invalid approach mode is setted")
return 'fail'

traj_msg_follower = PoseStamped()
traj_msg_follower.header.stamp = rospy.Time.now()
traj_msg_follower.pose.position.x = self.target_cog_pos[0]
traj_msg_follower.pose.position.y = self.target_cog_pos[1]
traj_msg_follower.pose.position.z = self.target_cog_pos[2]
target_att_qr = tf.transformations.quaternion_from_euler(self.target_att[0], self.target_att[1], self.target_att[2])
traj_msg_follower.pose.orientation.x = target_att_qr[0]
traj_msg_follower.pose.orientation.y = target_att_qr[1]
traj_msg_follower.pose.orientation.z = target_att_qr[2]
traj_msg_follower.pose.orientation.w = target_att_qr[3]

if self.leader_target_pose != self.leader_pre_target_pose:
self.leader_pre_target_pose = self.leader_target_pose
if(self.approach_mode == 'nav'):
self.follower_nav_pub.publish(nav_msg_follower)
elif(self.approach_mode == 'trajectory'):
self.follower_traj_pub.publish(traj_msg_follower)
else:
rospy.logerr("Invalid approach mode is setted")
return 'fail'

# roll and pitch
link_rot_follower = DesireCoord()
Expand Down Expand Up @@ -241,6 +240,7 @@ def leaderTargetPoseCb(self, msg):
leader_target_y = msg.y.target_p
leader_target_z = msg.z.target_p
leader_target_yaw = msg.yaw.target_p
self.leader_target_pose = [leader_target_x, leader_target_y, leader_target_z, leader_target_yaw]
leader_dock_point_2_cog_homo = np.eye(4)
follower_dock_point_2_cog_trans = None
leader_cog_2_world_home = self.coordTransformer.getHomoMatFromVector([leader_target_x, leader_target_y, leader_target_z], [0,0,leader_target_yaw])
Expand All @@ -249,8 +249,6 @@ def leaderTargetPoseCb(self, msg):
target_cp_pos = (leader_cog_2_world_home @ leader_dock_point_2_cog_homo @ self.target_offset)[:3]
self.target_att = np.array([0,0,leader_target_yaw])
self.target_cog_pos = target_cp_pos[:3]+np.array(tf.transformations.euler_matrix(self.target_att[0],self.target_att[1],self.target_att[2]))[:3, :3] @ np.array(follower_dock_point_2_cog_trans[0])
if not self.leader_target_pose_set_flag:
self.leader_target_pose_set_flag = True

class ApproachState(smach.State):
def __init__(self,
Expand Down Expand Up @@ -291,6 +289,7 @@ def __init__(self,
self.real_machine = real_machine
self.leader = leader
self.leader_id = leader_id
self.leader_pre_target_pose = []
self.target_offset = np.zeros(4)
self.x_offset = x_offset
self.y_offset = y_offset
Expand Down Expand Up @@ -319,7 +318,6 @@ def __init__(self,

# flags
self.emergency_flag = False
self.leader_target_pose_set_flag = False

# tf listener and broadcaster
self.listener = tf.TransformListener()
Expand Down Expand Up @@ -389,7 +387,6 @@ def execute(self, userdata):
elif np.all(np.greater(np.abs(pos_error),self.pos_danger_thre)) or np.all(np.greater(np.abs(att_error),self.att_danger_thre)):
return 'fail'
else:
# Nav(x,y,z and yaw)
nav_msg_follower = FlightNav()
nav_msg_follower.target = 1
nav_msg_follower.control_frame = 0
Expand All @@ -400,24 +397,28 @@ def execute(self, userdata):
nav_msg_follower.target_pos_y = self.target_cog_pos[1]
nav_msg_follower.target_pos_z = self.target_cog_pos[2]
nav_msg_follower.target_yaw = self.target_att[2]
# Trajectory(x,y,z and yaw)
# target_pos_cog = self.coordTransformer.posTransform('root','cog',target_pos)
# traj_msg_follower = PoseStamped()
# traj_msg_follower.header.stamp = rospy.Time.now()
# traj_msg_follower.pose.position.x = target_pos_cog[0]
# traj_msg_follower.pose.position.y = target_pos_cog[1]
# traj_msg_follower.pose.position.z = target_pos_cog[2]
# traj_msg_follower.pose.orientation.x = homo_transformed_target_odom[1][0]
# traj_msg_follower.pose.orientation.y = homo_transformed_target_odom[1][1]
# traj_msg_follower.pose.orientation.z = homo_transformed_target_odom[1][2]
# traj_msg_follower.pose.orientation.w = homo_transformed_target_odom[1][3]
if(self.approach_mode == 'nav'):
self.follower_nav_pub.publish(nav_msg_follower)
elif(self.approach_mode == 'trajectory'):
self.follower_traj_pub.publish(traj_msg_follower)
else:
rospy.logerr("Invalid approach mode is setted")
return 'fail'

traj_msg_follower = PoseStamped()
traj_msg_follower.header.stamp = rospy.Time.now()
traj_msg_follower.pose.position.x = self.target_cog_pos[0]
traj_msg_follower.pose.position.y = self.target_cog_pos[1]
traj_msg_follower.pose.position.z = self.target_cog_pos[2]
target_att_qr = tf.transformations.quaternion_from_euler(self.target_att[0], self.target_att[1], self.target_att[2])
traj_msg_follower.pose.orientation.x = target_att_qr[0]
traj_msg_follower.pose.orientation.y = target_att_qr[1]
traj_msg_follower.pose.orientation.z = target_att_qr[2]
traj_msg_follower.pose.orientation.w = target_att_qr[3]

if self.leader_target_pose != self.leader_pre_target_pose:
self.leader_pre_target_pose = self.leader_target_pose
if(self.approach_mode == 'nav'):
self.follower_nav_pub.publish(nav_msg_follower)
elif(self.approach_mode == 'trajectory'):
rospy.loginfo('okokookooko')
self.follower_traj_pub.publish(traj_msg_follower)
else:
rospy.logerr("Invalid approach mode is setted")
return 'fail'

# roll and pitch
link_rot_follower = DesireCoord()
Expand Down Expand Up @@ -445,6 +446,7 @@ def leaderTargetPoseCb(self, msg):
leader_target_y = msg.y.target_p
leader_target_z = msg.z.target_p
leader_target_yaw = msg.yaw.target_p
self.leader_target_pose = [leader_target_x, leader_target_y, leader_target_z, leader_target_yaw]
leader_dock_point_2_cog_homo = np.eye(4)
follower_dock_point_2_cog_trans = None
leader_cog_2_world_home = self.coordTransformer.getHomoMatFromVector([leader_target_x, leader_target_y, leader_target_z], [0,0,leader_target_yaw])
Expand All @@ -453,9 +455,6 @@ def leaderTargetPoseCb(self, msg):
target_cp_pos = (leader_cog_2_world_home @ leader_dock_point_2_cog_homo @ self.target_offset)[:3]
self.target_att = np.array([0,0,leader_target_yaw])
self.target_cog_pos = target_cp_pos[:3]+np.array(tf.transformations.euler_matrix(self.target_att[0],self.target_att[1],self.target_att[2]))[:3, :3] @ np.array(follower_dock_point_2_cog_trans[0])
if not self.leader_target_pose_set_flag:
self.leader_target_pose_set_flag = True

class AssemblyState(smach.State):
def __init__(self,
robot_name = 'ninja1',
Expand Down
12 changes: 8 additions & 4 deletions robots/ninja/script/tasks/assembly_motion.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,10 @@
from ninja.assembly_api import *

class AssemblyDemo():
def __init__(self,module_ids,real_machine):
def __init__(self,module_ids,real_machine,approach_mode):
self.module_ids = module_ids
self.real_machine = real_machine
self.approach_mode = approach_mode
rospy.loginfo(self.module_ids)
def main(self):
sm_top = smach.StateMachine(outcomes=['succeeded','interupted'])
Expand All @@ -35,7 +36,8 @@ def main(self):
leader = 'ninja'+str(target_leader_id),
leader_id = target_leader_id,
attach_dir = direction,
real_machine = self.real_machine),
real_machine = self.real_machine,
approach_mode = self.approach_mode),
transitions={'done':'ApproachState' + motion_prefix,
'in_process': 'StandbyState'+motion_prefix,
'emergency':'interupted_'+motion_prefix})
Expand All @@ -46,7 +48,8 @@ def main(self):
leader = 'ninja'+str(target_leader_id),
leader_id = target_leader_id,
attach_dir = direction,
real_machine = self.real_machine),
real_machine = self.real_machine,
approach_mode = self.approach_mode),
transitions={'done':'AssemblyState'+ motion_prefix,
'in_process':'ApproachState'+motion_prefix,
'fail':'StandbyState'+motion_prefix,
Expand Down Expand Up @@ -81,12 +84,13 @@ def main(self):
rospy.init_node("assembly_motion")
modules_str = rospy.get_param("module_ids", default="")
rm = rospy.get_param("real_machine", default=True)
approach_mode = rospy.get_param("approach_mode", default = "nav")
modules = []
if(modules_str):
modules = [int(x) for x in modules_str.split(',')]
else:
rospy.loginfo("No module ID is designated!")
try:
assembly_motion = AssemblyDemo(module_ids = modules, real_machine=rm);
assembly_motion = AssemblyDemo(module_ids = modules, real_machine=rm, approach_mode = approach_mode);
assembly_motion.main()
except rospy.ROSInterruptException: pass

0 comments on commit 2b1e5c6

Please sign in to comment.