def interpolate_joint_space(self, goal, total_time, nb_points, start=None): """ Interpolate a trajectory from a start state (or current state) to a goal in joint space :param goal: A RobotState to be used as the goal of the trajectory param total_time: The time to execute the trajectory :param nb_points: Number of joint-space points in the final trajectory :param start: A RobotState to be used as the start state, joint order must be the same as the goal :return: The corresponding RobotTrajectory """ dt = total_time*(1.0/nb_points) # create the joint trajectory message traj_msg = JointTrajectory() rt = RobotTrajectory() if start == None: start = self.get_current_state() joints = [] start_state = start.joint_state.position goal_state = goal.joint_state.position for j in range(len(goal_state)): pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1) joints.append(pose_lin[1:].tolist()) for i in range(nb_points): point = JointTrajectoryPoint() for j in range(len(goal_state)): point.positions.append(joints[j][i]) # append the time from start of the position point.time_from_start = rospy.Duration.from_sec((i+1)*dt) # append the position to the message traj_msg.points.append(point) # put name of joints to be moved traj_msg.joint_names = self.joint_names() # send the message rt.joint_trajectory = traj_msg return rt
def scale_trajectory_speed(traj, scale): # Create a new trajectory object new_traj = RobotTrajectory() # Initialize the new trajectory to be the same as the planned trajectory new_traj.joint_trajectory = traj.joint_trajectory # Get the number of joints involved n_joints = len(traj.joint_trajectory.joint_names) # Get the number of points on the trajectory n_points = len(traj.joint_trajectory.points) # Store the trajectory points points = list(traj.joint_trajectory.points) # Cycle through all points and scale the time from start, speed and acceleration for i in range(n_points): point = JointTrajectoryPoint() point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale point.velocities = list(traj.joint_trajectory.points[i].velocities) point.accelerations = list(traj.joint_trajectory.points[i].accelerations) point.positions = traj.joint_trajectory.points[i].positions for j in range(n_joints): point.velocities[j] = point.velocities[j] * scale point.accelerations[j] = point.accelerations[j] * scale * scale points[i] = point # Assign the modified points to the new trajectory new_traj.joint_trajectory.points = points # Return the new trajectory return new_traj
def display(self, trajectory): """ Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin :param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message """ # Publish the DisplayTrajectory (only for trajectory preview in RViz) # includes a convert of float durations in rospy.Duration() def js_to_rt(js): rt = RobotTrajectory() rt.joint_trajectory.joint_names = js.name rt.joint_trajectory.points.append(JointTrajectoryPoint(positions=js.position)) return rt dt = DisplayTrajectory() if isinstance(trajectory, RobotTrajectory): dt.trajectory.append(trajectory) elif isinstance(trajectory, JointTrajectory): rt = RobotTrajectory() rt.joint_trajectory = trajectory dt.trajectory.append(rt) elif isinstance(trajectory, RobotState): dt.trajectory.append(js_to_rt(trajectory.joint_state)) elif isinstance(trajectory, JointState): dt.trajectory.append(js_to_rt(trajectory)) else: raise NotImplementedError("ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}".format(str(type(trajectory)))) self._display_traj.publish(dt)
def to_robot_trajectory(self, num_waypoints=300, jointspace=True): """ Parameters ---------- num_waypoints : float how many points in the :obj:`moveit_msgs.msg.RobotTrajectory` jointspace : bool What kind of trajectory. Joint space points are 7x' and describe the angle of each arm. Workspace points are 3x', and describe the x,y,z position of the end effector. """ traj = JointTrajectory() traj.joint_names = self.limb.joint_names() points = [] for t in np.linspace(0, self.total_time, num=num_waypoints): point = self.trajectory_point(t, jointspace) points.append(point) # We want to make a final point at the end of the trajectory so that the # controller has time to converge to the final point. extra_point = self.trajectory_point(self.total_time, jointspace) extra_point.time_from_start = rospy.Duration.from_sec(self.total_time + 1) points.append(extra_point) traj.points = points traj.header.frame_id = 'base' robot_traj = RobotTrajectory() robot_traj.joint_trajectory = traj return robot_traj
def scale_trajectory_speed(traj, scale): new_traj = RobotTrajectory() new_traj.joint_trajectory = traj.joint_trajectory n_joints = len(traj.joint_trajectory.joint_names) n_points = len(traj.joint_trajectory.points) points = list(traj.joint_trajectory.points) for i in range(n_points): point = JointTrajectoryPoint() point.positions = traj.joint_trajectory.points[i].positions point.time_from_start = traj.joint_trajectory.points[ i].time_from_start / scale point.velocities = list(traj.joint_trajectory.points[i].velocities) point.accelerations = list( traj.joint_trajectory.points[i].accelerations) for j in range(n_joints): point.velocities[j] = point.velocities[j] * scale point.accelerations[j] = point.accelerations[j] * scale * scale points[i] = point new_traj.joint_trajectory.points = points return new_traj
def dict_to_trajectory(plan): plan_msg = RobotTrajectory() joint_traj = JointTrajectory() joint_traj.header.frame_id = plan["joint_trajectory"]["frame_id"] joint_traj.joint_names = plan["joint_trajectory"]["joint_names"] for point in plan["joint_trajectory"]["points"]: joint_traj.points.append( JointTrajectoryPoint(positions=point["positions"], velocities=point["velocities"], accelerations=point["accelerations"], time_from_start=point["time_from_start"])) multi_dof_joint_traj = MultiDOFJointTrajectory() multi_dof_joint_traj.header.frame_id = plan["multi_dof_joint_trajectory"][ "frame_id"] multi_dof_joint_traj.joint_names = plan["multi_dof_joint_trajectory"][ "joint_names"] for point in plan["multi_dof_joint_trajectory"]["points"]: multi_dof_joint_traj_point = MultiDOFJointTrajectoryPoint() for t in point["transforms"]: multi_dof_joint_traj_point.transforms.append(list_to_transform(t)) multi_dof_joint_traj_point.time_from_start = point["time_from_start"] multi_dof_joint_traj.points.append(multi_dof_joint_traj_point) plan_msg.joint_trajectory = joint_traj plan_msg.multi_dof_joint_trajectory = multi_dof_joint_traj return plan_msg
def run_joint_trajectory(self, joint_trajectory): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. """ plan = RobotTrajectory() plan.joint_trajectory = joint_trajectory self._move_group_commander.execute(plan)
def get_display_trajectory(self, *joint_trajectories): display_trajectory = DisplayTrajectory() display_trajectory.model_id = 'pr2' for joint_trajectory in joint_trajectories: robot_trajectory = RobotTrajectory() robot_trajectory.joint_trajectory = joint_trajectory # robot_trajectory.multi_dof_joint_trajectory = ... display_trajectory.trajectory.append(robot_trajectory) display_trajectory.trajectory_start = self.get_robot_state() return display_trajectory
def cascade_plans(plan1, plan2): ''' Joins two robot motion plans into one inputs: two robot trajactories outputs: final robot trjactory ''' # Create a new trajectory object new_traj = RobotTrajectory() # Initialize the new trajectory to be the same as the planned trajectory traj_msg = JointTrajectory() # Get the number of joints involved n_joints1 = len(plan1.joint_trajectory.joint_names) n_joints2 = len(plan2.joint_trajectory.joint_names) # Get the number of points on the trajectory n_points1 = len(plan1.joint_trajectory.points) n_points2 = len(plan2.joint_trajectory.points) # Store the trajectory points points1 = list(plan1.joint_trajectory.points) points2 = list(plan2.joint_trajectory.points) end_time = plan1.joint_trajectory.points[n_points1 - 1].time_from_start start_time = plan1.joint_trajectory.points[0].time_from_start duration = end_time - start_time # add a time toleracne between successive plans time_tolerance = rospy.Duration.from_sec(0.1) for i in range(n_points1): point = JointTrajectoryPoint() point.time_from_start = plan1.joint_trajectory.points[ i].time_from_start point.velocities = list(plan1.joint_trajectory.points[i].velocities) point.accelerations = list( plan1.joint_trajectory.points[i].accelerations) point.positions = plan1.joint_trajectory.points[i].positions points1[i] = point traj_msg.points.append(point) end_time = plan1.joint_trajectory.points[i].time_from_start for i in range(n_points2): point = JointTrajectoryPoint() point.time_from_start = plan2.joint_trajectory.points[i].time_from_start + \ end_time + time_tolerance point.velocities = list(plan2.joint_trajectory.points[i].velocities) point.accelerations = list( plan2.joint_trajectory.points[i].accelerations) point.positions = plan2.joint_trajectory.points[i].positions traj_msg.points.append(point) traj_msg.joint_names = plan1.joint_trajectory.joint_names traj_msg.header.frame_id = plan1.joint_trajectory.header.frame_id new_traj.joint_trajectory = traj_msg return new_traj
def slow_down(traj): """Slows down an exiting move it carestian path trajectory Slows down a moveit trajectory by a factor defined in ``spd``. Iterates through all points in trajectory and scales the following variables: ``time_from_start``, ``accelerations``, ``positions`` Args: traj (RobotTrajectory): Standard ROS message type from moveit_msgs.msg Returns: RobotTrajectory: identical but slow downed RobotTrajectory object """ new_traj = RobotTrajectory() new_traj.joint_trajectory = traj.joint_trajectory n_joints = len(traj.joint_trajectory.joint_names) n_points = len(traj.joint_trajectory.points) spd = 0.2 # Lower this when using the Gazebo Robot for i in range(n_points): new_traj.joint_trajectory.points[ i].time_from_start = traj.joint_trajectory.points[ i].time_from_start / spd # rospy.loginfo(type(traj.joint_trajectory.points[i])) v = list(new_traj.joint_trajectory.points[i].velocities) a = list(new_traj.joint_trajectory.points[i].accelerations) p = list(new_traj.joint_trajectory.points[i].positions) for j in range(n_joints): # rospy.loginfo(type(new_traj.joint_trajectory.points[i].velocities)) v[j] = traj.joint_trajectory.points[i].velocities[j] * spd a[j] = traj.joint_trajectory.points[i].accelerations[j] * spd**2 p[j] = traj.joint_trajectory.points[i].positions[j] # new_traj.joint_trajectory.points[i].accelerations[j] = traj.joint_trajectory.points[i].accelerations[j] * spd # new_traj.joint_trajectory.points[i].positions[j] = traj.joint_trajectory.points[i].positions[j] v = tuple(v) a = tuple(a) p = tuple(p) new_traj.joint_trajectory.points[i].velocities = v new_traj.joint_trajectory.points[i].accelerations = a new_traj.joint_trajectory.points[i].positions = p # rospy.loginfo( new_traj.joint_trajectory.points[i].velocities[j]) # rospy.loginfo( new_traj.joint_trajectory.points[i].accelerations[j]) # rospy.loginfo( new_traj.joint_trajectory.points[i].positions[j]) return new_traj
def create_trajectory(self, q_list): jt = JointTrajectory() jt.header.frame_id = self.frame_id jt.points = [] for i in range(0, len(q_list)): p = JointTrajectoryPoint() p.positions = q_list[i, :] p.time_from_start.secs = i / 10. p.time_from_start.nsecs = i / 10. + 0.002 jt.points.append(p) jt.joint_names = self.joint_names rt = RobotTrajectory() rt.joint_trajectory = jt return rt # (rt for move_group; jt for motion_planning)
def create_display_traj(self, joint_traj): self.display_traj = DisplayTrajectory() self.display_traj.model_id = 'lbr4' robot_traj = RobotTrajectory() robot_traj.joint_trajectory = joint_traj self.display_traj.trajectory.append(robot_traj) self.display_traj.trajectory_start.\ joint_state.header.frame_id = '/world' self.display_traj.trajectory_start.joint_state.name = \ self.kdl_kin.get_joint_names() self.display_traj.trajectory_start.joint_state.position = \ joint_traj.points[0].positions self.display_traj.trajectory_start.joint_state.velocity = \ joint_traj.points[0].velocities
def viz_joint_traj(self, j_traj, base_link='base_link'): robot_tr = RobotTrajectory() j_traj.header.frame_id = base_link robot_tr.joint_trajectory = j_traj disp_tr = DisplayTrajectory() disp_tr.trajectory.append(robot_tr) disp_tr.model_id = self.robot_name i = 0 while (not rospy.is_shutdown() and i < 10): i += 1 self.pub_tr.publish(disp_tr) self.rate.sleep()
def _convertTrajectory(self, trajectory): points = [] for wp in trajectory.waypoints: timeStamp = rostime.Duration(secs=wp.timestamp[0], nsecs=wp.timestamp[1]) jtp = JointTrajectoryPoint(positions=wp.positions, velocities=wp.velocities, accelerations=wp.accelerations, time_from_start=timeStamp) points.append(jtp) jointTraj = JointTrajectory(joint_names=trajectory.joint_names, points=points) robotTraj = RobotTrajectory() robotTraj.joint_trajectory = jointTraj return robotTraj
def scale_trajectory_speed(traj, scale): # Create a new trajectory object new_traj = RobotTrajectory() # Initialize the new trajectory to be the same as the input trajectory new_traj.joint_trajectory = traj.joint_trajectory # Get the number of joints involved n_joints = len(traj.joint_trajectory.joint_names) # Get the number of points on the trajectory n_points = len(traj.joint_trajectory.points) # Store the trajectory points points = list(traj.joint_trajectory.points) # Cycle through all points and joints and scale the time from start, # as well as joint speed and acceleration for i in range(n_points): point = JointTrajectoryPoint() # The joint positions are not scaled so pull them out first point.positions = traj.joint_trajectory.points[i].positions # Next, scale the time_from_start for this point point.time_from_start = traj.joint_trajectory.points[ i].time_from_start / scale # Get the joint velocities for this point point.velocities = list(traj.joint_trajectory.points[i].velocities) # Get the joint accelerations for this point point.accelerations = list( traj.joint_trajectory.points[i].accelerations) # Scale the velocity and acceleration for each joint at this point for j in range(n_joints): point.velocities[j] = point.velocities[j] * scale point.accelerations[j] = point.accelerations[j] * scale * scale # Store the scaled trajectory point points[i] = point # Assign the modified points to the new trajectory new_traj.joint_trajectory.points = points # Return the new trajecotry return new_traj
def handle_execute_request(self, req): if "manipulator" in req.REQUEST_TYPE: self.group = moveit_commander.MoveGroupCommander("manipulator") elif "gripper" in req.REQUEST_TYPE: self.group = moveit_commander.MoveGroupCommander("gripper") else: rospy.logwarn("PlanningService::handle_plan_request() -- requested group is not recognized") return False #build moveit msg for display joint_multi_traj_msg = MultiDOFJointTrajectory() robot_traj_msg = RobotTrajectory() robot_traj_msg.joint_trajectory = req.JOINT_TRAJECTORY robot_traj_msg.multi_dof_joint_trajectory = joint_multi_traj_msg self.group.execute(robot_traj_msg, wait=True)
def handle_show_request(self, req): display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state() #build moveit msg for display joint_multi_traj_msg = MultiDOFJointTrajectory() robot_traj_msg = RobotTrajectory() robot_traj_msg.joint_trajectory = req.JOINT_TRAJECTORY robot_traj_msg.multi_dof_joint_trajectory = joint_multi_traj_msg display_trajectory.trajectory.append(robot_traj_msg) rospy.loginfo("PlanningService::handle_show_request() -- showing trajectory %s", req.JOINT_TRAJECTORY) self.display_trajectory_publisher.publish(display_trajectory); return True
def set_trajectory_speed(traj, speed): # Create a new trajectory object new_traj = RobotTrajectory() # Initialize the new trajectory to be the same as the input trajectory new_traj.joint_trajectory = traj.joint_trajectory # Get the number of joints involved n_joints = len(traj.joint_trajectory.joint_names) # Get the number of points on the trajectory n_points = len(traj.joint_trajectory.points) # Store the trajectory points points = list(traj.joint_trajectory.points) # Cycle through all points and joints and scale the time from start, # as well as joint speed and acceleration time_list = [] for i in range(n_points): point = JointTrajectoryPoint() # The joint positions are not scaled so pull them out first point.positions = traj.joint_trajectory.points[i].positions # Next, scale the time_from_start for this point point.time_from_start = traj.joint_trajectory.points[i].time_from_start t = rospy.Time(point.time_from_start.secs, point.time_from_start.nsecs) # ipdb.set_trace() time_list.append(t.to_time()) # Initialize the joint velocities for this point point.velocities = [speed] * n_joints # Get the joint accelerations for this point point.accelerations = [speed / 4.0] * n_joints # Store the scaled trajectory point points[i] = point # Assign the modified points to the new trajectory new_traj.joint_trajectory.points = points # ipdb.set_trace() # plt.plot(time_list) # plt.show() # Return the new trajecotry return new_traj
def scale_trajectory_speed(traj, scale): # Create a new trajectory object new_traj = RobotTrajectory() # Initialize the new trajectory to be the same as the input trajectory new_traj.joint_trajectory = traj.joint_trajectory # Get the number of joints involved n_joints = len(traj.joint_trajectory.joint_names) # Get the number of points on the trajectory n_points = len(traj.joint_trajectory.points) # Store the trajectory points points = list(traj.joint_trajectory.points) # Cycle through all points and joints and scale the time from start, # as well as joint speed and acceleration for i in range(n_points): point = JointTrajectoryPoint() # The joint positions are not scaled so pull them out first point.positions = traj.joint_trajectory.points[i].positions # Next, scale the time_from_start for this point point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale # Get the joint velocities for this point point.velocities = list(traj.joint_trajectory.points[i].velocities) # Get the joint accelerations for this point point.accelerations = list(traj.joint_trajectory.points[i].accelerations) # Scale the velocity and acceleration for each joint at this point for j in range(n_joints): point.velocities[j] = point.velocities[j] * scale point.accelerations[j] = point.accelerations[j] * scale * scale # Store the scaled trajectory point points[i] = point # Assign the modified points to the new trajectory new_traj.joint_trajectory.points = points # Return the new trajecotry return new_traj
def convertPlanToTrajectory(path): """ Convert a path in OMPL to a trajectory in Moveit that could be used for display; :param plan: A path in OMPL; :return: a RobotTrajectory message in Moveit; """ trajectory = RobotTrajectory() states = path.getStates() points = [] joint_trajectory = JointTrajectory() joint_trajectory.joint_names = get_joint_names('right') for state in states: point = JointTrajectoryPoint() point.positions = convertStateToRobotState(state).joint_state.position points.append(point) joint_trajectory.points = points trajectory.joint_trajectory = joint_trajectory return trajectory
def reverse_planning(self, plan): rev_plan = RobotTrajectory() rev_joint_traj = JointTrajectory() rev_joint_traj.header = copy.deepcopy(plan.joint_trajectory.header) rev_joint_traj.joint_names = copy.deepcopy(plan.joint_trajectory.joint_names) new_points = [] points = plan.joint_trajectory.points # JointTrajectoryPoint[] l = len(points) ''' How to reverse the cartesian plan? Reverse the order of trajectory point's joint values. Change the sign of velocity and acceleration. Change the accumulative time parameterization to follow the reverse order time gap. But you should put the current joint value to the first trajectory point. If not, the planner will return error due to joint deviation. ''' for i in range(l): point = JointTrajectoryPoint() if i == 0: point.positions = self.ur5.get_current_joint_values() point.velocities = [0.0 for val in point.positions] point.accelerations = [0.0 for val in point.positions] point.time_from_start = rospy.Duration(0) else: point.positions = copy.deepcopy(points[l-1-i].positions) point.velocities = [-value for value in points[l-1-i].velocities] point.accelerations = [-value for value in points[l-1-i].accelerations] point.time_from_start = new_points[i-1].time_from_start + points[l-i].time_from_start - points[l-1-i].time_from_start new_points.append(point) rev_joint_traj.points = new_points rev_plan.joint_trajectory = rev_joint_traj return rev_plan
def visualize_trajectory(self, blocking=True): if not self.display: return if not self.single_arm: self.adjust_traj_length() if self.rviz_pub.get_num_connections() < 1: rospy.logerr("Rviz topic not subscribed") return msg = DisplayTrajectory() msg.trajectory_start = self.robot_model.get_current_state() traj = RobotTrajectory() goal = JointTrajectory() goal.joint_names = self.left.JOINT_NAMES[:] if not self.single_arm: goal.joint_names += self.right.JOINT_NAMES[:] # The sim is very slow if the number of points is too large steps = 1 if len(self.left._goal.trajectory.points) < 100 else 10 for idx in range(0, len(self.left._goal.trajectory.points), steps): comb_point = JointTrajectoryPoint() lpt = self.left._goal.trajectory.points[idx] comb_point.positions = lpt.positions[:] if not self.single_arm: comb_point.positions += self.right._goal.trajectory.points[idx].positions[:] comb_point.time_from_start = lpt.time_from_start goal.points.append(comb_point) traj.joint_trajectory = goal msg.trajectory.append(traj) duration = goal.points[-1].time_from_start.to_sec() self.rviz_pub.publish(msg) if blocking: rospy.loginfo("Waiting for trajectory animation {} seconds".format(duration)) rospy.sleep(duration)
def robotTrajectoryFromPlanPoseBased(self, plan, groups, downsample_freq=None): """Given a dmp plan (GetDMPPlanResponse) create a RobotTrajectory doing IK calls for the PoseStampeds provided so we can visualize and execute the motion""" rt = RobotTrajectory() jt = JointTrajectory() #jt.joint_names jt.points # Magic code goes here. fjtg = self.computeJointTrajFromCartesian(plan.plan.points, plan.plan.times, downsample_freq=downsample_freq) rt.joint_trajectory = fjtg.trajectory #plan.plan.times should be the times used if len(rt.joint_trajectory.points) != len(plan.plan.times): rospy.logerr("joint trajectory point does not have same length than planned times, this means there are IKs that failed") rospy.logerr("points: " + str(len(rt.joint_trajectory.points)) + " times: " + str(len(plan.plan.times))) # for point, time in zip(rt.joint_trajectory.points, plan.plan.times): # # Probably need to allocate it again here... # point.time_from_start = rospy.Duration(time) return rt
def plan(self, joints=None): """ Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """ if type(joints) is JointState: self._g.set_joint_value_target(joints.position) elif type(joints) is Pose: self.set_pose_target(joints) elif not joints == None: try: self.set_joint_value_target(self.get_remembered_joint_values()[joints]) except: self.set_joint_value_target(joints) plan = self._g.get_plan() plan_msg = RobotTrajectory() joint_traj = JointTrajectory() joint_traj.joint_names = plan["joint_trajectory"]["joint_names"] for point in plan["joint_trajectory"]["points"]: joint_traj.points.append( JointTrajectoryPoint( positions=point["positions"], velocities=point["velocities"], accelerations=point["accelerations"] ) ) multi_dof_joint_traj = MultiDOFJointTrajectory() multi_dof_joint_traj.joint_names = plan["multi_dof_joint_trajectory"]["joint_names"] multi_dof_joint_traj.frame_ids = plan["multi_dof_joint_trajectory"]["frame_ids"] multi_dof_joint_traj.child_frame_ids = plan["multi_dof_joint_trajectory"]["child_frame_ids"] for point in plan["multi_dof_joint_trajectory"]["points"]: multi_dof_joint_traj_point = MultiDOFJointTrajectoryPoint() for t in point["transforms"]: multi_dof_joint_traj_point.poses.append( Transform( translation=Vector(x=t["translation"]["x"], y=t["translation"]["y"], z=t["translation"]["z"]), rotation=Quaternion( x=t["rotation"]["x"], y=t["rotation"]["y"], z=t["rotation"]["z"], w=t["rotation"]["w"] ), ) ) multi_dof_joint_traj.points.append(multi_dof_joint_traj_point) plan_msg.joint_trajectory = joint_traj plan_msg.multi_dof_joint_trajectory = multi_dof_joint_traj return plan_msg
def createRobotTrajectoryFromJointStates(joint_states, interesting_joint_list): """Given a joint_states message and the joints we are interested in moving create a RobotTrajectory that satisfies it""" curr_joint_states = copy.deepcopy(joint_states) rt = RobotTrajectory() jt = JointTrajectory() jt_point = JointTrajectoryPoint() jt.joint_names = interesting_joint_list jt.header.stamp = rospy.Time.now() for joint in interesting_joint_list: # Search for the position of the joint in the joint_states message idx_joint = curr_joint_states.name.index(joint) jt_point.positions.append(curr_joint_states.position[idx_joint]) jt_point.velocities.append(0.0) # Set speeds to 0.0, in theory movements will be little and also with no effort jt_point.time_from_start = rospy.Duration(0.3) # Arbitrary time, we are not dealing with speed issues here jt.points.append(jt_point) rt.joint_trajectory = jt return rt
def create_trajectory(self, q_list, v_list, a_list, t): tnsec, tsec = np.modf(t) jt = JointTrajectory() jt.header.frame_id = '/panda_link0' jt.points = [] for i, tm in enumerate(t): p = JointTrajectoryPoint() p.positions = q_list[i, :] p.velocities = v_list[i, :] p.accelerations = a_list[i, :] p.time_from_start.secs = tsec[i] p.time_from_start.nsecs = tnsec[i] * 1e09 jt.points.append(p) jt.joint_names = [ "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7" ] rt = RobotTrajectory() rt.joint_trajectory = jt return rt # (rt for move_group; jt for motion_planning)
def send_traj(self, u_arr): # Formulate joint trajectory message: jt_msg = JointTrajectory() jt_msg.joint_names = self.joint_names for i in range(len(u_arr)): u = u_arr[i] jt_pt = JointTrajectoryPoint() jt_pt.positions = u jt_msg.points.append(jt_pt) robot_tr = RobotTrajectory() robot_tr.joint_trajectory = jt_msg disp_tr = DisplayTrajectory() disp_tr.trajectory.append(robot_tr) disp_tr.model_id = self.robot_name self.pub_tr.publish(disp_tr) i = 0 while (not rospy.is_shutdown() and i < 10): i += 1 self.rate.sleep()
def single_shot(path, obstacles): exp = DiffCoplusBaxterExperiments() print('Adding box') rospy.sleep(2) box_names = [] ## Commented out because assuming obstacles already exist in scene # for i, obs in enumerate(obstacles): # box_name = 'box_{}'.format(i) # box_names.append(box_name) # box_pose = geometry_msgs.msg.PoseStamped() # box_pose.header.frame_id = exp.planning_frame # # box_pose.pose.orientation.w = 1.0 # box_pose.pose.position.x = obs[1][0] # box_pose.pose.position.y = obs[1][1] # box_pose.pose.position.z = obs[1][2] # exp.scene.add_box(box_name, box_pose, size=obs[2]) # wait_for_state_update(exp.scene, box_name, box_is_known=True) pub = exp.display_trajectory_publisher joint_traj = JointTrajectory() for q in path: traj_point = JointTrajectoryPoint() traj_point.positions = q.numpy().tolist() joint_traj.points.append(traj_point) joint_traj.joint_names = [ 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2' ] robot_traj = RobotTrajectory() robot_traj.joint_trajectory = joint_traj disp_traj = DisplayTrajectory() disp_traj.trajectory.append(robot_traj) disp_traj.trajectory_start.joint_state.position = path[0].numpy() disp_traj.trajectory_start.joint_state.name = joint_traj.joint_names pub.publish(disp_traj) return box_names, exp
def scale_trajectory_speed(traj, scale): # Create a new trajectory object new_traj = RobotTrajectory() # Initialize the new trajectory to be the same as the planned trajectory new_traj.joint_trajectory = traj.joint_trajectory # Get the number of joints involved n_joints = len(traj.joint_trajectory.joint_names) # Get the number of points on the trajectory n_points = len(traj.joint_trajectory.points) # Store the trajectory points points = list(traj.joint_trajectory.points) # Cycle through all points and scale the time from start, speed and acceleration for i in range(n_points): point = JointTrajectoryPoint() point.time_from_start = traj.joint_trajectory.points[ i].time_from_start / scale point.velocities = list(traj.joint_trajectory.points[i].velocities) point.accelerations = list( traj.joint_trajectory.points[i].accelerations) point.positions = traj.joint_trajectory.points[i].positions for j in range(n_joints): point.velocities[j] = point.velocities[j] * scale point.accelerations[j] = point.accelerations[j] * scale * scale points[i] = point # Assign the modified points to the new trajectory new_traj.joint_trajectory.points = points # Return the new trajecotry return new_traj
def dict_to_trajectory(plan): plan_msg = RobotTrajectory() joint_traj = JointTrajectory() joint_traj.header.frame_id = plan["joint_trajectory"]["frame_id"] joint_traj.joint_names = plan["joint_trajectory"]["joint_names"] for point in plan["joint_trajectory"]["points"]: joint_traj.points.append(JointTrajectoryPoint( positions = point["positions"], velocities = point["velocities"], accelerations = point["accelerations"], time_from_start = rospy.Duration().from_sec(point["time_from_start"]))) multi_dof_joint_traj = MultiDOFJointTrajectory() multi_dof_joint_traj.header.frame_id = plan["multi_dof_joint_trajectory"]["frame_id"] multi_dof_joint_traj.joint_names = plan["multi_dof_joint_trajectory"]["joint_names"] for point in plan["multi_dof_joint_trajectory"]["points"]: multi_dof_joint_traj_point = MultiDOFJointTrajectoryPoint() for t in point["transforms"]: multi_dof_joint_traj_point.transforms.append(list_to_transform(t)) multi_dof_joint_traj_point.time_from_start = rospy.Duration().from_sec(point["time_from_start"]) multi_dof_joint_traj.points.append(multi_dof_joint_traj_point) plan_msg.joint_trajectory = joint_traj plan_msg.multi_dof_joint_trajectory = multi_dof_joint_traj return plan_msg
def plan(self, joints = None): """ Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """ if type(joints) is JointState: self._g.set_joint_value_target(joints.position) elif type(joints) is Pose: self.set_pose_target(joints) elif not joints == None: try: self.set_joint_value_target(self.get_remembered_joint_values()[joints]) except: self.set_joint_value_target(joints) plan = self._g.get_plan() plan_msg = RobotTrajectory() joint_traj = JointTrajectory() joint_traj.joint_names = plan["joint_trajectory"]["joint_names"] for point in plan["joint_trajectory"]["points"]: joint_traj.points.append(JointTrajectoryPoint( positions = point["positions"], velocities = point["velocities"], accelerations = point["accelerations"])) multi_dof_joint_traj = MultiDOFJointTrajectory() multi_dof_joint_traj.joint_names = plan["multi_dof_joint_trajectory"]["joint_names"] multi_dof_joint_traj.frame_ids = plan["multi_dof_joint_trajectory"]["frame_ids"] multi_dof_joint_traj.child_frame_ids = plan["multi_dof_joint_trajectory"]["child_frame_ids"] for point in plan["multi_dof_joint_trajectory"]["points"]: multi_dof_joint_traj_point = MultiDOFJointTrajectoryPoint() for t in point["transforms"]: multi_dof_joint_traj_point.poses.append(Transform( translation = Vector(x = t["translation"]["x"], y = t["translation"]["y"], z = t["translation"]["z"]), rotation = Quaternion(x = t["rotation"]["x"], y = t["rotation"]["y"], z = t["rotation"]["z"], w = t["rotation"]["w"]))) multi_dof_joint_traj.points.append(multi_dof_joint_traj_point) plan_msg.joint_trajectory = joint_traj plan_msg.multi_dof_joint_trajectory = multi_dof_joint_traj return plan_msg
def create_tracking_trajectory_1(self, traj, speed, min_speed): # Create a new trajectory object new_traj = RobotTrajectory() # Initialize the new trajectory to be the same as the input trajectory new_traj.joint_trajectory = deepcopy(traj.joint_trajectory) # Get the number of joints involved n_joints = len(traj.joint_trajectory.joint_names) # Get the number of points on the trajectory n_points = len(traj.joint_trajectory.points) # Cycle through all points and joints and scale the time from start, # as well as joint speed and acceleration for i in range(n_points): # The joint positions are not scaled so pull them out first new_traj.joint_trajectory.points[ i].positions = traj.joint_trajectory.points[i].positions # Next, scale the time_from_start for this point new_traj.joint_trajectory.points[ i].time_from_start = traj.joint_trajectory.points[ i].time_from_start / speed # Initialize the joint velocities for this point new_traj.joint_trajectory.points[i].velocities = [ speed + min_speed ] * n_joints # Get the joint accelerations for this point new_traj.joint_trajectory.points[i].accelerations = [speed / 4.0 ] * n_joints # Return the new trajecotry return new_traj
def display_trajectory(self, plan): print(plan) traj = plan.trajectory joints = traj.joint_names print(traj) init_joints = JointState() init_joints.name = joints init_joints.position = traj.points[0].positions init_joints.velocity = traj.points[0].velocities init_joints.effort = traj.points[0].effort joint_traj = RobotTrajectory() joint_traj.joint_trajectory = plan.trajectory init_state = RobotState() init_state.joint_state = init_joints display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = init_state display_trajectory.trajectory.append(joint_traj) # Publish self.display_trajectory_publisher.publish(display_trajectory) rospy.sleep(3)
def execute_plan_close_loop_with_moveit(arm_cmd_pub, gripper_cmd_pub, args): from moveit_msgs.msg import RobotTrajectory # ** execution of plan ** # open gripper before path gripper_openning(gripper_cmd_pub) rospy.loginfo("openning gripper...") arm_cmd = args['pre_grasp_trajectory'] # construct plan arm_traj = RobotTrajectory() arm_traj.joint_trajectory = arm_cmd #arm_cmd = add_velocity(arm_cmd) group.execute(arm_traj, wait=True) #arm_cmd_pub.publish(arm_cmd) rospy.loginfo("pre_grasp executing...") #arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.01, ori_dif_threshold=10) #arm_block_until_near(end_pose=args['pre_grasp_end_pose']) arm_cmd = args['pre_to_grasp_trajectory'] # construct plan arm_traj = RobotTrajectory() arm_traj.joint_trajectory = arm_cmd #arm_cmd = add_velocity(arm_cmd) group.execute(arm_traj, wait=True) #arm_cmd_pub.publish(arm_cmd) rospy.loginfo("pre_grasp to grasp executing...") #arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.001, ori_dif_threshold=10) #arm_block_until_near(end_pose=args['grasp_end_pose']) rospy.loginfo("closing gripper...") gripper_closing(gripper_cmd_pub) #gripper_closing_and_store_pose(model_name, global_grasp_pose, grasp_id) # post_grasp arm_cmd = args['post_grasp_trajectory'] #arm_cmd = add_velocity(arm_cmd) # construct plan arm_traj = RobotTrajectory() arm_traj.joint_trajectory = arm_cmd #arm_cmd = add_velocity(arm_cmd) group.execute(arm_traj, wait=True) #arm_cmd_pub.publish(arm_cmd) rospy.loginfo("post_grasp executing...") #arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.01, ori_dif_threshold=10) #arm_block_until_near(end_pose=args['post_grasp_end_pose']) # execute plan for placing arm_cmd = args['place_trajectory'] #arm_cmd = add_velocity(arm_cmd) # construct plan arm_traj = RobotTrajectory() arm_traj.joint_trajectory = arm_cmd #arm_cmd = add_velocity(arm_cmd) group.execute(arm_traj, wait=True) #arm_cmd_pub.publish(arm_cmd) rospy.loginfo("place plan executing...") #arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.01, ori_dif_threshold=10) #arm_block_until_near(end_pose=args['place_end_pose']) # detect if the object didn't slip, and the grasp is successful store_pose_based_on_gripper(args['global_grasp_pose'], args['grasp_id']) # detect if gripper is closed gripper_success = detect_gripper_closing() # open gripper gripper_openning(gripper_cmd_pub) rospy.sleep(0.5) rospy.loginfo("openning gripper...") # execute plan for resetting # post_place_execute arm_cmd = args['post_place_trajectory'] #arm_cmd = add_velocity(arm_cmd) # construct plan arm_traj = RobotTrajectory() arm_traj.joint_trajectory = arm_cmd #arm_cmd = add_velocity(arm_cmd) group.execute(arm_traj, wait=True) #arm_cmd_pub.publish(arm_cmd) rospy.loginfo("post-place plan executing...") #arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.01, ori_dif_threshold=10) #arm_block_until_near(end_pose=args['post_place_end_pose']) arm_cmd = args['reset_trajectory'] #arm_cmd = add_velocity(arm_cmd) # construct plan arm_traj = RobotTrajectory() arm_traj.joint_trajectory = arm_cmd #arm_cmd = add_velocity(arm_cmd) group.execute(arm_traj, wait=True) #arm_cmd_pub.publish(arm_cmd) rospy.loginfo("reset plan executing...") #arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.01, ori_dif_threshold=10) return gripper_success
def get_robot_trajectory_msg(self, path_config): robot_trajectory = RobotTrajectory() robot_trajectory.joint_trajectory = self.get_joint_trajectory_msg( path_config) return robot_trajectory
def convert_jointspace_to_workspace_trajectory(joint_traj, limb, kin, num_ways=300): """ PROJECT 1 PART B Converts a jointspace RobotTrajectory into a workspace trajectory. Recall that for workspace trajectories, we store positions as SE(3) configurations given in the form of a 7D vector, where the first 3 entries are the spatial x,y,z position, and the final 4 entries are the quaternion representation of the orientation. """ raise NotImplementedError # Interpolates the given joint trajectory to have num_ways waypoints. The result is an # array of times and joint_positions (where the robot will be at those respective times). # This is done because the trajectories returned by moveit tend to be quite sparse. times, joint_positions = interpolate_path(joint_traj.joint_trajectory, num_ways=num_ways) traj = JointTrajectory() traj.joint_names = limb.joint_names() points = [] for i in range(len(joint_positions) - 1): curr_pos = joint_positions[i] # current vector of joint angles. next_pos = joint_positions[i + 1] # next vector of joint angles. # Compute workspace configuration and body velocity. # hint: kin.forward_position_kinematics, get_g_matrix, and g_matrix_log may be useful. curr_workspace_config = TODO if times[i + 1] - times[i] > 1e-5: body_velocity = TODO else: body_velocity = np.zeros(6) point = JointTrajectoryPoint() point.positions = curr_workspace_config point.velocities = body_velocity point.time_from_start = rospy.Duration(times[i]) points.append(point) # We want to make a final point at the end of the trajectory so that the # controller has time to converge to the final point. last_point = JointTrajectoryPoint() last_pos = joint_traj.joint_trajectory.points[-1].positions last_time = joint_traj.joint_trajectory.points[-1].time_from_start positions_dict = joint_array_to_dict(last_pos, limb) last_workspace_config = kin.forward_position_kinematics( joint_values=positions_dict) last_point.positions = last_workspace_config # What should the desired body velocity at this final position? last_point.velocities = TODO # TODO last_point.time_from_start = last_time points.append(last_point) traj.points = points traj.header.frame_id = 'base' robot_traj = RobotTrajectory() robot_traj.joint_trajectory = traj return robot_traj
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) # Connect to the right_arm move group right_arm = moveit_commander.MoveGroupCommander('arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Display the name of the end_effector link rospy.loginfo("The end effector link is: " + str(end_effector_link)) # Set a small tolerance on joint angles right_arm.set_goal_tolerance(0.0001) right_arm.set_named_target('right') right_arm.go() rospy.sleep(1) ''' start_pose = right_arm.get_current_pose().pose start_pose.position.x = 0.4 start_pose.position.y = 0 #start_pose.position.z = 0.4 start_pose.orientation.x = 0.707106781186547 start_pose.orientation.y = 0 start_pose.orientation.z = 0 start_pose.orientation.w = 0.707106781186547 right_arm.set_pose_target(start_pose) right_arm.go() # Pause for a moment rospy.sleep(1) ''' plan = RobotTrajectory() joint_trajectory = JointTrajectory() joint_trajectory.header.frame_id = 'base_link' #joint_trajectory.header.stamp = rospy.Time.now() joint_trajectory.joint_names = [ 'base_to_armA', 'armA_to_armB', 'armB_to_armC', 'armC_to_armD' ] # Output with open("/home/jyk/Desktop/pvt.txt", 'r') as f: for line in f.readlines(): cont = line.rstrip('\r\n').replace(' ', '').split(',') point = JointTrajectoryPoint() point.positions = map(float, cont[0:4]) point.velocities = map(float, cont[4:8]) point.time_from_start = rospy.Duration(float(cont[8])) joint_trajectory.points.append(deepcopy(point)) plan.joint_trajectory = joint_trajectory #print type(plan) #print plan right_arm.set_start_state_to_current_state() right_arm.execute(plan) #rospy.sleep(3) ''' # Return the arm to the named "resting" pose stored in the SRDF file right_arm.set_named_target('right') right_arm.go() rospy.sleep(1) ''' # Cleanly shut down MoveIt moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def create_tracking_trajectory(self, traj, speed, min_speed): # Create a new trajectory object new_traj = RobotTrajectory() # Initialize the new trajectory to be the same as the input trajectory new_traj.joint_trajectory = deepcopy(traj.joint_trajectory) # Get the number of joints involved n_joints = len(traj.joint_trajectory.joint_names) # Get the number of points on the trajectory n_points = len(traj.joint_trajectory.points) # Get the current joint values current_joint_values = self.right_arm.get_current_joint_values() # Compute the difference from the final joint values delta_joint_values = np.subtract( traj.joint_trajectory.points[n_points - 1].positions, tuple(current_joint_values)) delta_signs = list() for i in range(n_joints): delta_signs.append(copysign(1.0, delta_joint_values[i])) traj_position_array = np.array( traj.joint_trajectory.points[n_points - 1].positions) traj_position_array += np.array(delta_signs) * 0.05 # Pull off the final joint state #self.right_arm.set_joint_value_target(traj.joint_trajectory.points[n_points - 1].positions) self.right_arm.set_joint_value_target(traj_position_array) new_traj = self.right_arm.plan() for i in range(1): # The joint positions are not scaled so pull them out first #new_traj.joint_trajectory.points[i].positions = traj.joint_trajectory.points[i].positions + tuple([0.1] * n_joints) # Next, scale the time_from_start for this point new_traj.joint_trajectory.points[ i].time_from_start = new_traj.joint_trajectory.points[ i].time_from_start / speed # Initialize the joint velocities for this point new_traj.joint_trajectory.points[i].velocities = [ speed + min_speed ] * n_joints # Get the joint accelerations for this point new_traj.joint_trajectory.points[i].accelerations = [speed / 4.0 ] * n_joints #new_traj.joint_trajectory.points = new_traj.joint_trajectory.points[n_points - 1:] # Next, scale the time_from_start for this point #new_traj.joint_trajectory.points[0].time_from_start = traj.joint_trajectory.points[n_points - 1].time_from_start / speed # Initialize the joint velocities for this point #new_traj.joint_trajectory.points[0].velocities = [speed + min_speed] * n_joints # Get the joint accelerations for this point #new_traj.joint_trajectory.points[0].accelerations = [speed / 4.0] * n_joints # # Cycle through all points and joints and scale the time from start, as well as joint speed and acceleration # for i in range(n_points): # # The joint positions are not scaled so pull them out first # new_traj.joint_trajectory.points[i].positions = traj.joint_trajectory.points[i].positions + tuple([0.1] * n_joints) # # # Next, scale the time_from_start for this point # new_traj.joint_trajectory.points[i].time_from_start = traj.joint_trajectory.points[i].time_from_start / speed # # # Initialize the joint velocities for this point # new_traj.joint_trajectory.points[i].velocities = [speed + min_speed] * n_joints # # # Get the joint accelerations for this point # new_traj.joint_trajectory.points[i].accelerations = [speed / 4.0] * n_joints # Return the new trajecotry return new_traj