def execute_trajectories(self): trajectory_goals, tmp = self._get_trajectory_and_end_effector_goals_from_waypoints( ) # execute initial position-to-start trajectory if len(trajectory_goals) == 0: print("Error: No trajectory goals to execute!") else: # execute initial position to start trajectory init_goal = TrajectoryGoal() waypoint_1 = WaypointMsg() waypoint_1.names = self.hebi_mapping waypoint_1.positions = self._get_joint_angles() waypoint_1.velocities = [0] * len(self.hebi_mapping) waypoint_1.accelerations = [0] * len(self.hebi_mapping) waypoint_2 = trajectory_goals[0].waypoints[0] init_goal.waypoints = [waypoint_1, waypoint_2] init_goal.times = [0, 3] self._executing_trajectory = True self.trajectory_action_client.send_goal(init_goal) self.trajectory_action_client.wait_for_result() # execute trajectory goals for goal in trajectory_goals: self.trajectory_action_client.send_goal(goal) self.trajectory_action_client.wait_for_result() self.trajectory_action_client.get_result() self._executing_trajectory = False
def execute(self, ud): self.enter(ud) init_wp = WaypointMsg() lift_wp = WaypointMsg() end_wp = WaypointMsg() eff_prev_target_pos = self.kdl_fk.forward( ud.prev_joint_pos)[:3, 3].reshape(1, 3).tolist()[0] jt_init_pos = self.hebi_wrap.get_joint_positions() eff_init_pos = self.kdl_fk.forward(jt_init_pos)[:3, 3].reshape( 1, 3).tolist()[0] eff_target_pos = [ ud.target_end_link_point.x, ud.target_end_link_point.y, ud.target_end_link_point.z ] # init_wp init_wp.names = self.hebi_wrap.hebi_mapping init_wp.positions = jt_init_pos init_wp.velocities = [0.0] * self.hebi_wrap.hebi_count init_wp.accelerations = [0.0] * self.hebi_wrap.hebi_count # lift_wp lift_wp.names = self.hebi_wrap.hebi_mapping eff_lift_pos = [(a + b + 2.0 * c) / 2.0 for a, b, c in zip( eff_init_pos, eff_target_pos, self.step_height_vector)] success, lift_wp.positions = self._get_pos_ik( self.trac_ik, jt_init_pos, eff_lift_pos, seed_xyz=eff_prev_target_pos) lift_wp.velocities = [NAN] * self.hebi_wrap.hebi_count lift_wp.accelerations = [NAN] * self.hebi_wrap.hebi_count # end_wp end_wp.names = self.hebi_wrap.hebi_mapping success, end_wp.positions = self._get_pos_ik(self.trac_ik, lift_wp.positions, eff_target_pos, seed_xyz=eff_lift_pos) end_wp.velocities = [0.0] * self.hebi_wrap.hebi_count end_wp.accelerations = [0.0] * self.hebi_wrap.hebi_count goal = TrajectoryGoal() goal.waypoints = [init_wp, lift_wp, end_wp] goal.times = [0.0, ud.execution_time / 2.0, ud.execution_time] # send goal to trajectory action server self._hold_leg_position = False self.hebi_wrap.trajectory_action_client.send_goal(goal) self.hebi_wrap.trajectory_action_client.wait_for_result() self._hold_leg_position = True ud.prev_joint_pos = end_wp.positions self.exit(ud) return 'success'
def get_traj_goal_to_target_jt_pos(target_jt_pos, time): goal = TrajectoryGoal() i_wp = WaypointMsg() i_wp.names = hebi_mapping_flat i_wp.positions = get_jt_angles() i_wp.velocities = [0.0]*len(hebi_mapping_flat) i_wp.accelerations = [0.0]*len(hebi_mapping_flat) f_wp = WaypointMsg() f_wp.positions = target_jt_pos f_wp.velocities = [0.0]*len(hebi_mapping_flat) f_wp.accelerations = [0.0]*len(hebi_mapping_flat) goal.waypoints = [i_wp, f_wp] goal.times = [0, time] return goal
def generateGoal(self): self.goal = TrajectoryGoal() self.goal.times = self.interp_times self.goal.waypoints = [] for i in range(0, self.num_interp_waypoints): waypoint = WaypointMsg() waypoint.names = self.names waypoint.velocities = [NaN, NaN, NaN, NaN] waypoint.accelerations = [NaN, NaN, NaN, NaN] workspace_waypoint = [] # construct workspace waypoint for j in range(0, self.num_workspace_dof): workspace_waypoint.append( \ self.workspace_waypoints[j][i]) # get elbow up elbow = self.elbow_up[0] for j in range(0, self.num_waypoints): if (self.interp_times[i] <= self.times[j]): elbow = self.elbow_up[j] # run ik to get configspace waypoint configuration_waypoint = kin.ik(workspace_waypoint, elbow) waypoint.positions = configuration_waypoint self.goal.waypoints.append(waypoint) self.goal.waypoints[0].velocities = [0, 0, 0, 0] self.goal.waypoints[0].accelerations = [0, 0, 0, 0] self.goal.waypoints[-1].velocities = [0, 0, 0, 0] self.goal.waypoints[-1].accelerations = [0, 0, 0, 0]
def _get_trajectory_goal_from_json_data(trajectory_json): """Converts Python object into hebiros TrajectoryGoal() Args: trajectory_json (obj): Python object from customized json.load(...) with an custom object_hook function Returns: obj: TrajectoryGoal() with current joint state appended to front """ # Get data from json data structure waypoints = [] for i in range(len(trajectory_json["waypoints"])): waypoints.append(trajectory_json["waypoints"][str(i)]) times = trajectory_json["times"] # Create ROS msg trajectory_goal = TrajectoryGoal() for waypoint in waypoints: waypoint_msg = WaypointMsg() waypoint_msg.names = waypoint["names"] waypoint_msg.positions = waypoint["positions"] waypoint_msg.velocities = waypoint["velocities"] waypoint_msg.accelerations = waypoint["accelerations"] trajectory_goal.waypoints.append(waypoint_msg) trajectory_goal.times = times return trajectory_goal
def stand_up(self): rospy.loginfo("Hexapod standing up...") current_leg_positions = self._get_joint_angles() goal = TrajectoryGoal() start_wp = WaypointMsg() start_wp.names = self.hebi_mapping_flat start_wp.positions = self._flatten(current_leg_positions) start_wp.velocities = [0.0] * ACTUATORS_TOTAL start_wp.accelerations = [0.0] * ACTUATORS_TOTAL goal.waypoints.append(start_wp) goal.times.append(0.0) end_wp = WaypointMsg() end_wp.names = self.hebi_mapping_flat end_wp.positions = self._flatten(self.leg_jt_home_pos) end_wp.velocities = [0.0] * ACTUATORS_TOTAL end_wp.accelerations = [0.0] * ACTUATORS_TOTAL goal.waypoints.append(end_wp) goal.times.append(4.0) self.trajectory_action_client.send_goal( goal) # TODO: Add the various callbacks self.trajectory_action_client.wait_for_result() self.hold_pos([1, 2, 3, 4, 5, 6])
def _get_trajectory_and_end_effector_goals_from_waypoints(self): trajectory_goals = [] end_effector_goals = [] prev_breakpoint = False prev_time = 0.0 for waypoint in self.waypoints: if waypoint is not None: if len(trajectory_goals) == 0 or prev_breakpoint: trajectory_goals.append(TrajectoryGoal()) end_effector_goals.append([]) # if applicable, 1st append last waypoint from previous trajectory if len(trajectory_goals) > 1 and prev_breakpoint: waypoint_msg = WaypointMsg() waypoint_msg.names = list( trajectory_goals[-2].waypoints[-1].names) waypoint_msg.velocities = [0] * len(waypoint_msg.names) waypoint_msg.accelerations = [0] * len(waypoint_msg.names) trajectory_goals[-1].waypoints.append(waypoint_msg) trajectory_goals[-1].times.append(0.0) end_effector_goals[-1].append( copy.deepcopy(end_effector_goals[-2])) # append a new waypoint trajectory_goals[-1].waypoints.append( copy.deepcopy(waypoint.joint_state)) prev_time += waypoint.time trajectory_goals[-1].times.append(prev_time) end_effector_goals[-1].append( copy.deepcopy(waypoint.end_effector_pose)) prev_breakpoint = False else: # breakpoint prev_time = 0.0 prev_breakpoint = True return trajectory_goals, end_effector_goals
def __init__(self, hebi_group_name, hebi_mapping, hebi_gains, urdf_str, base_link, end_link): rospy.loginfo("Creating {} instance".format(self.__class__.__name__)) self.openmeta_testbench_manifest_path = rospy.get_param('~openmeta/testbench_manifest_path', None) if self.openmeta_testbench_manifest_path is not None: self._testbench_manifest = load_json_file(self.openmeta_testbench_manifest_path) # TestBench Parameters self._params = {} for tb_param in self._testbench_manifest['Parameters']: self._params[tb_param['Name']] = tb_param['Value'] # WARNING: If you use these values - make sure to check the type # TestBench Metrics self._metrics = {} for tb_metric in self._testbench_manifest['Metrics']: # FIXME: Hmm, this is starting to look a lot like OpenMDAO... self._metrics[tb_metric['Name']] = tb_metric['Value'] if hebi_gains is None: hebi_gains = { 'p': [float(self._params['p_gain'])]*3, 'i': [float(self._params['i_gain'])]*3, 'd': [float(self._params['d_gain'])]*3 } hebi_families = [] hebi_names = [] for actuator in hebi_mapping: family, name = actuator.split('/') hebi_families.append(family) hebi_names.append(name) rospy.loginfo(" hebi_group_name: %s", hebi_group_name) rospy.loginfo(" hebi_families: %s", hebi_families) rospy.loginfo(" hebi_names: %s", hebi_names) self.hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names) # Set PID Gains cmd_msg = CommandMsg() cmd_msg.name = hebi_mapping cmd_msg.settings.name = hebi_mapping cmd_msg.settings.control_strategy = [4, 4, 4] cmd_msg.settings.position_gains.name = hebi_mapping cmd_msg.settings.position_gains.kp = hebi_gains['p'] cmd_msg.settings.position_gains.ki = hebi_gains['i'] cmd_msg.settings.position_gains.kd = hebi_gains['d'] cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me. self.hebi_wrap.send_command_with_acknowledgement(cmd_msg) if base_link is None or end_link is None: robot = Robot.from_xml_string(urdf_str) if not base_link: base_link = robot.get_root() # WARNING: There may be multiple leaf nodes if not end_link: end_link = [x for x in robot.link_map.keys() if x not in robot.child_map.keys()][0] # pykdl self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link) self._active_joints = self.kdl_fk.get_joint_names() # joint data self.position_fbk = [0.0]*self.hebi_wrap.hebi_count self.velocity_fbk = [0.0]*self.hebi_wrap.hebi_count self.effort_fbk = [0.0]*self.hebi_wrap.hebi_count # joint state publisher while not rospy.is_shutdown() and len(self.hebi_wrap.get_joint_positions()) < len(self.hebi_wrap.hebi_mapping): rospy.sleep(0.1) self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) self.hebi_wrap.add_feedback_callback(self._joint_state_cb) # Set up Waypoint/Trajectory objects self.start_wp = WaypointMsg() self.start_wp.names = self.hebi_wrap.hebi_mapping self.end_wp = copy.deepcopy(self.start_wp) self.goal = TrajectoryGoal() self.goal.waypoints = [self.start_wp, self.end_wp] self._hold_positions = [0.0]*3 self._hold = True self.jointstate = JointState() self.jointstate.name = self.hebi_wrap.hebi_mapping rospy.sleep(1.0)
def loop(self): """Main Hexapod loop (distant - somewhat less accomplished - relative of HEBI algorithm) - Get chassis translation - Get leg end-effector translations (relative to leg base link) - side_alpha:odd, side_beta:even or side_alpha:even, side_beta:odd - side_alpha legs lift, plant to +transformation - side_alpha legs push to new home positions; side_beta legs push to -transformation - swap side_alpha and side_beta """ rospy.loginfo("Hexapod entering main loop...") rospy.loginfo( " Waiting for initial velocity command on /hexapod/cmd_vel/ ...") while self.last_vel_cmd is None: self._loop_rate.sleep() # start main loop while not rospy.is_shutdown(): chassis_pos_delta = None if self.last_vel_cmd is not None: dt = 1 # FIXME: Temporary for debugging lin_disp_lmt = self.linear_displacement_limit ang_disp_lmt = self.angular_displacement_limit chassis_pos_delta = Twist() chassis_pos_delta.linear.x = clamp( self.last_vel_cmd.linear.x * dt, -lin_disp_lmt, lin_disp_lmt) chassis_pos_delta.linear.y = clamp( self.last_vel_cmd.linear.y * dt, -lin_disp_lmt, lin_disp_lmt) chassis_pos_delta.linear.z = clamp( self.last_vel_cmd.linear.z * dt, -lin_disp_lmt, lin_disp_lmt) chassis_pos_delta.angular.x = clamp( self.last_vel_cmd.angular.x * dt, -ang_disp_lmt, ang_disp_lmt) chassis_pos_delta.angular.y = clamp( self.last_vel_cmd.angular.y * dt, -ang_disp_lmt, ang_disp_lmt) chassis_pos_delta.angular.z = clamp( self.last_vel_cmd.angular.z * dt, -ang_disp_lmt, ang_disp_lmt) self.last_vel_cmd = None if chassis_pos_delta is not None \ and not self._check_if_twist_msg_is_zero(chassis_pos_delta, linear_threshold=0.005, angular_threshold=0.01): # Get chassis position transformation chassis_pos_rot = transforms.euler_matrix( chassis_pos_delta.angular.x, chassis_pos_delta.angular.y, chassis_pos_delta.angular.z)[:3, :3] rospy.loginfo("chassis_pos_rot: %s", chassis_pos_rot) chassis_pos_trans = np.zeros([3]) chassis_pos_trans[0] = chassis_pos_delta.linear.x chassis_pos_trans[1] = chassis_pos_delta.linear.y chassis_pos_trans[2] = chassis_pos_delta.linear.z chassis_translation = np.dot(chassis_pos_trans, chassis_pos_rot) rospy.loginfo("chassis_translation: %s", chassis_translation) leg_target_eff_translation = [[]] * LEGS # Get leg base positions relative to chassis leg_base_positions = self._get_base_to_leg_base_fk() for i, leg_base_pos in enumerate(leg_base_positions): leg_base_pos_arr = np.array(leg_base_pos).reshape(3, 1) leg_base_pos_arr_new = np.dot(chassis_pos_rot, leg_base_pos_arr) leg_base_pos_trans_4 = np.ones(4).reshape(4, 1) leg_base_pos_trans_4[:3, :] = leg_base_pos_arr_new # get leg base translations relative to leg_base coordinate frame relative_trans = np.dot( np.linalg.inv(self.kdl_fk_base_to_leg_base[i].forward( [])), leg_base_pos_trans_4) relative_trans = relative_trans.reshape(1, 4).tolist()[0][:3] leg_target_eff_translation[i] = relative_trans # Get leg target end-effector translations for i, q in enumerate(self.leg_jt_home_pos): base_to_leg_base_rot = self.kdl_fk_base_to_leg_base[ i].forward([])[:3, :3] leg_target_eff_trans = np.dot( np.linalg.inv(base_to_leg_base_rot), chassis_translation).tolist()[0] leg_target_eff_translation[i] = [ x + y for x, y in zip(leg_target_eff_translation[i], leg_target_eff_trans) ] # TODO: FIXME: Technically incorrect # 1: side_alpha legs lift, plant to +transformation rospy.loginfo( "1: side_alpha legs lift, plant to +transformation") if self._odd_starts: active_legs = [1, 2, 5] else: # even starts active_legs = [0, 3, 4] init_wp = WaypointMsg() lift_wp = WaypointMsg() end_wp = WaypointMsg() legs_jt_init_pos = self._get_joint_angles() leg_eff_cur_pos = self._get_leg_base_to_eff_fk( legs_jt_init_pos) for i in range(LEGS): motor_names = [name for name in self.hebi_mapping[i]] # INITIAL POSITION init_wp.names.extend(motor_names) init_wp.positions.extend(legs_jt_init_pos[i]) init_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) init_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) # LIFT lift_wp.names.extend(motor_names) if i in active_legs: # apply translation leg_lift_eff_target_pos = [ (x + y + z) / 2.0 for x, y, z in zip( leg_eff_cur_pos[i], self.leg_eff_home_pos[i], leg_target_eff_translation[i]) ] leg_lift_eff_target_pos = [ x + y for x, y in zip(leg_lift_eff_target_pos, self.leg_eff_step_height[i]) ] # get ik leg_lift_jt_target_pos = self._get_pos_ik( self.trac_ik_leg_base_to_end[i], legs_jt_init_pos[i], leg_lift_eff_target_pos, seed_xyz=self.leg_eff_home_pos[i]) lift_wp.positions.extend(leg_lift_jt_target_pos) lift_wp.velocities.extend([NAN] * ACTUATORS_PER_LEG) lift_wp.accelerations.extend([NAN] * ACTUATORS_PER_LEG) else: lift_wp.positions.extend(legs_jt_init_pos[i]) lift_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) lift_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) # PLANT end_wp.names.extend(motor_names) if i in active_legs: # apply translation leg_plant_eff_target_pos = [ x + y for x, y in zip(self.leg_eff_home_pos[i], leg_target_eff_translation[i]) ] leg_plant_eff_target_pos[2] = self.leg_eff_home_pos[i][ 2] # end eff z-position should match home z-position # get ik leg_plant_jt_target_pos = self._get_pos_ik( self.trac_ik_leg_base_to_end[i], leg_lift_jt_target_pos, leg_plant_eff_target_pos, seed_xyz=leg_lift_eff_target_pos) end_wp.positions.extend(leg_plant_jt_target_pos) end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) else: end_wp.positions.extend(legs_jt_init_pos[i]) end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) goal = TrajectoryGoal() goal.waypoints.append(init_wp) goal.waypoints.append(lift_wp) goal.waypoints.append(end_wp) goal.times.extend([0.0, 0.4, 0.8]) self.release_pos([1, 2, 3, 4, 5, 6]) self.trajectory_action_client.send_goal(goal) self.trajectory_action_client.wait_for_result() self.hold_pos([1, 2, 3, 4, 5, 6]) # 2: side_alpha legs push to new home positions; side_beta legs push to -transformation rospy.loginfo( "2: side_alpha legs push to new home positions; side_beta legs push to -transformation" ) if self._odd_starts: active_legs = [0, 3, 4] else: # even starts active_legs = [1, 2, 5] init_wp = WaypointMsg() end_wp = WaypointMsg() legs_jt_init_pos = self._get_joint_angles() for i in range(LEGS): motor_names = [name for name in self.hebi_mapping[i]] # INITIAL POSITION init_wp.names.extend(motor_names) init_wp.positions.extend(legs_jt_init_pos[i]) init_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) init_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) # PUSH end_wp.names.extend(motor_names) if i in active_legs: # apply -translation leg_plant_eff_target_pos = [ x + y for x, y in zip(self.leg_eff_home_pos[i], [ -val for val in leg_target_eff_translation[i] ]) ] leg_plant_eff_target_pos[2] = self.leg_eff_home_pos[i][ 2] # end eff z-position should match home z-position # get ik leg_plant_jt_target_pos = self._get_pos_ik( self.trac_ik_leg_base_to_end[i], legs_jt_init_pos[i], leg_plant_eff_target_pos, seed_xyz=self.leg_eff_home_pos[i]) end_wp.positions.extend(leg_plant_jt_target_pos) end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) else: end_wp.positions.extend(self.leg_jt_home_pos[i]) end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) goal = TrajectoryGoal() goal.waypoints.append(init_wp) goal.waypoints.append(end_wp) goal.times.extend([0.0, 0.4]) self.release_pos([1, 2, 3, 4, 5, 6]) self.trajectory_action_client.send_goal(goal) self.trajectory_action_client.wait_for_result() self.hold_pos([1, 2, 3, 4, 5, 6]) self._odd_starts = not self._odd_starts self._loop_rate.sleep( ) # FIXME: Doesn't make sense to use this unless re-planning trajectories
def main(): # Initialize ROS node rospy.init_node("example_04_trajectory_node", disable_signals=True) rate = rospy.Rate(200) group_name = "my_group" num_joints = 3 num_waypoints = 5 # Create a client which uses the service to create a group add_group_client = rospy.ServiceProxy("hebiros/add_group_from_names", AddGroupFromNamesSrv) # Create a subscriber to receive feedback from a group # Register feedback callback which runs when feedback is received feedback_subscriber = rospy.Subscriber("/hebiros/"+group_name+"/feedback/joint_state", JointState, feedback_callback, queue_size=100) # Construct a group using 3 known modules rospy.wait_for_service("hebiros/add_group_from_names") req_group_name = group_name req_names = ["base", "shoulder", "elbow"] req_families = ["HEBI"] add_group_client(req_group_name, req_names, req_families) # Create an action client for executing a trajectory client = SimpleActionClient("/hebiros/"+group_name+"/trajectory", TrajectoryAction) # Wait for the action server corresponding to the action client client.wait_for_server() user_input = raw_input("Press Enter/Return to execute Trajectory") # Construct a trajectory to be sent as an action goal goal = TrajectoryGoal() # Set the times to reach each waypoint in seconds times = [0, 5, 10, 15, 20] names = ["HEBI/base", "HEBI/shoulder", "HEBI/elbow"] # Wait for feedback from actuators while not rospy.is_shutdown() and feedback.position is not None and len(feedback.position) < len(names): rate.sleep() # Set positions, velocities, and accelerations for each waypoint and each joint # The following vectors have one joint per row and one waypoint per column positions = [[feedback.position[0], 0, M_PI_2, 0, 0], [feedback.position[1], 0, M_PI_2, M_PI_2, 0], [feedback.position[2], 0, 0, M_PI_2, 0]] velocities = [[0, NAN, NAN, NAN, 0], [0, NAN, NAN, NAN, 0], [0, NAN, NAN, NAN, 0]] accelerations = [[0, NAN, NAN, NAN, 0], [0, NAN, NAN, NAN, 0], [0, NAN, NAN, NAN, 0]] # Construct the goal using the TrajectoryGoal format for i in range(num_waypoints): waypoint = WaypointMsg() waypoint.names = names waypoint.positions = [joint[i] for joint in positions] waypoint.velocities = [joint[i] for joint in velocities] waypoint.accelerations = [joint[i] for joint in accelerations] goal.waypoints.append(waypoint) goal.times.append(times[i]) # Send the goal, executing the trajectory client.send_goal(goal, trajectory_done, trajectory_active, trajectory_feedback) while not rospy.is_shutdown(): rate.sleep()
def move_to_pose(self, target_pose, mode, move_duration=0): """ :param target_pose: [x,y,z,R,P,Y] or Pose msg :type target_pose: list or Pose :param mode: "trajectory" or "command" :type mode: str :param move_duration: time in seconds to complete move :type move_duration: float :return: """ xyz_bounds = [0.01, 0.01, 0.01] wxyz_bounds = [0.05, 0.05, 0.05] if not isinstance(target_pose, Pose): # xyz target_xyz = [0, 0, 0] for i in range(3): if target_pose[i] in ("", " "): xyz_bounds[i] = 1e5 else: target_xyz[i] = float(target_pose[i]) # RPY target_rpy = [0, 0, 0] # Tait-Byran Extrinsic xyz Euler angles for i in range(3): if target_pose[3 + i] in ("", " "): wxyz_bounds[i] = 1e5 else: target_rpy[i] = float(target_pose[3 + i]) # wxyz target_wxyz = transforms.quaternion_from_euler( target_rpy[0], target_rpy[1], target_rpy[2], axes='sxyz').tolist() else: target_xyz = [ target_pose.position.x, target_pose.position.y, target_pose.position.z ] target_wxyz = [ target_pose.orientation.w, target_pose.orientation.x, target_pose.orientation.y, target_pose.orientation.z ] print("Seed angles: {}".format( [round(ang, 4) for ang in self.hebi_wrap.get_joint_positions()])) print("Target End-Effector Pose...") print(" xyz: {}".format(target_xyz)) print(" wxyz: {}".format(target_wxyz)) print("Bounds...") print(" xyz: {}".format(xyz_bounds)) print(" wxyz: {}".format(wxyz_bounds)) # Get target joint angles target_jt_angles = self.trac_ik.get_ik( self.hebi_wrap.get_joint_positions(), target_xyz[0], target_xyz[1], target_xyz[2], target_wxyz[0], target_wxyz[1], target_wxyz[2], target_wxyz[3], xyz_bounds[0], xyz_bounds[1], xyz_bounds[2], wxyz_bounds[0], wxyz_bounds[1], wxyz_bounds[2]) if target_jt_angles is None: print("NO JOINT SOLUTION FOUND FOR END-EFFECTOR POSE " "\n(x: {}, y: {}, z: {}, W: {}, X: {}, Y: {}, Z: {})" "\nPLEASE TRY AGAIN.".format(*target_xyz + target_wxyz)) return False else: if mode == "trajectory": goal = TrajectoryGoal() start_wp = WaypointMsg() start_wp.names = self.hebi_mapping start_wp.positions = self.hebi_wrap.get_joint_positions() start_wp.velocities = [0] * len(self.hebi_mapping) start_wp.accelerations = [0] * len(self.hebi_mapping) goal.waypoints.append(start_wp) end_wp = WaypointMsg() end_wp.names = self.hebi_mapping end_wp.positions = target_jt_angles end_wp.velocities = [0] * len(self.hebi_mapping) end_wp.accelerations = [0] * len(self.hebi_mapping) goal.waypoints.append(end_wp) goal.times.extend([0, move_duration]) self._hold_position = False self.hebi_wrap.trajectory_action_client.send_goal_and_wait( goal) self._hold_joint_angles = self.hebi_wrap.get_joint_positions() self._hold_position = True else: # mode == "command" cmd = JointState() cmd.name = self.hebi_mapping cmd.position = target_jt_angles cmd.velocity = [0] * len(self.hebi_mapping) cmd.effort = [0] * len(self.hebi_mapping) self._hold_position = False self.hebi_wrap.joint_state_publisher.publish(cmd) self._hold_joint_angles = target_jt_angles self._hold_position = True return True