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 main(): pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory) rospy.init_node('traj_test') p1 = JointTrajectoryPoint() p1.positions = [0,0,0,0,0,0,0] p1.velocities = [0,0,0,0,0,0,0] p1.accelerations = [0,0,0,0,0,0,0] p2 = JointTrajectoryPoint() p2.positions = [0,0,1.0,0,-1.0,0,0] p2.velocities = [0,0,0,0,0,0,0] p2.accelerations = [0,0,0,0,0,0,0] p2.time_from_start = rospy.Time(4.0) p3 = JointTrajectoryPoint() p3.positions = [0,0,-1.0,0,1.0,0,0] p3.velocities = [0,0,0,0,0,0,0] p3.accelerations = [0,0,0,0,0,0,0] p3.time_from_start = rospy.Time(20.0) traj = JointTrajectory() traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7'] traj.points = [p1,p2,p3] rospy.loginfo(traj) r = rospy.Rate(1) # 10hz pub.publish(traj) r.sleep() pub.publish(traj)
def add_point(self, positions, velocities, accelerations, time): point = JointTrajectoryPoint() point.positions = copy(positions) point.velocities = copy(velocities) point.accelerations = copy(accelerations) point.time_from_start = rospy.Duration(time) self._goal.trajectory.points.append(point)
def make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id='/base_link'): ''' This function will convert the plan to a joint trajectory compatible with the /arm_N/arm_controller/command topic ''' theplan = plan # create joint trajectory message jt = JointTrajectory() # fill the header jt.header.seq = seq jt.header.stamp.secs = 0 #secs jt.header.stamp.nsecs = 0 #nsecs jt.header.frame_id = frame_id # specify the joint names jt.joint_names = theplan.joint_trajectory.joint_names # fill the trajectory points and computer constraint times njtp = len(theplan.joint_trajectory.points) for ii in range(0, njtp): jtp = JointTrajectoryPoint() jtp = copy.deepcopy(theplan.joint_trajectory.points[ii]) jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0] jtp.accelerations =[0.0, 0.0, 0.0, 0.0, 0.0] jtp.time_from_start = rospy.Duration.from_sec(secs + dt*(ii+1)) jt.points.append(jtp) return jt
def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict): if self._mode == "velocity": velocities = [0.0] * len(joint_names) cmd = dict(zip(joint_names, velocities)) while not self._server.is_new_goal_available() and self._alive: self._limb.set_joint_velocities(cmd) if self._cuff_state: self._limb.exit_control_mode() break rospy.sleep(1.0 / self._control_rate) elif self._mode == "position" or self._mode == "position_w_id": raw_pos_mode = self._mode == "position_w_id" if raw_pos_mode: pnt = JointTrajectoryPoint() pnt.positions = self._get_current_position(joint_names) if dimensions_dict["velocities"]: pnt.velocities = [0.0] * len(joint_names) if dimensions_dict["accelerations"]: pnt.accelerations = [0.0] * len(joint_names) while not self._server.is_new_goal_available() and self._alive: self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode) # zero inverse dynamics feedforward command if self._mode == "position_w_id": pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time) ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt) self._pub_ff_cmd.publish(ff_pnt) if self._cuff_state: self._limb.exit_control_mode() break rospy.sleep(1.0 / self._control_rate)
def solution2hand_joint_path(solution, robot, total_time): trajectory = JointTrajectory() trajectory.joint_names = ["left_hand_synergy_joint"] n_waypoints = len(solution['joint_trajectory']) time_sec = 0.0001 time_incr = float(total_time) / n_waypoints for i in range(n_waypoints): #extrapolation = 1.1 joint_cmd = np.max(solution['joint_trajectory'][i]) if i >= n_waypoints - 2: joint_cmd = np.minimum(joint_cmd * 1.5, 1.0) else: joint_cmd = joint_cmd rospy.loginfo("Joint command %d = %f" % (i, joint_cmd)) joint_positions = [joint_cmd] point = JointTrajectoryPoint() point.positions = joint_positions point.velocities = [0.0] * len(joint_positions) point.accelerations = [0.0] * len(joint_positions) point.time_from_start = rospy.Duration(time_sec) trajectory.points.append(point) time_sec += time_incr return trajectory
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 runTrajectory(req): global Traj_server; print "---------------------------------" print req.task print " " print req.bin_num print " " print req.using_torso print "---------------------------------" if req.using_torso == "y": file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num); else: file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num); if req.task == "Forward": file_name = file_root+"/forward"; elif req.task == "Drop": file_name = file_root+"/drop"; else : return taskResponse(False); f = open(file_name,"r"); plan_file = RobotTrajectory(); buf = f.read(); plan_file.deserialize(buf); plan = copy.deepcopy(plan_file); arm_right_group = moveit_commander.MoveGroupCommander("arm_right"); arm_left_group = moveit_commander.MoveGroupCommander("arm_left"); arm_left_group.set_start_state_to_current_state(); StartPnt = JointTrajectoryPoint(); StartPnt.positions = arm_left_group.get_current_joint_values(); StartPnt.velocities = [0]*len(StartPnt.positions); StartPnt. accelerations = [0]*len(StartPnt.positions); #print StartPnt; plan.joint_trajectory.points[0] = StartPnt; #print plan; display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory) display_trajectory = moveit_msgs.msg.DisplayTrajectory(); robot = moveit_commander.RobotCommander(); display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while", file_name, " is visualized (again)..." arm_left_group.execute(plan); print "Trajectory ", file_name, " finished!" f.close(); return taskResponse(True);
def move_joint_traj(self, traj, dt): goal = FollowJointTrajectoryActionGoal() goal.goal.trajectory.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] for i in range(len(traj)): point = JointTrajectoryPoint() p = [0] * 6 v = [0] * 6 last_v = [0] * 6 a = [0] * 6 if i != len(traj) - 1 and i != 0: for j in range(6): p[j] = traj[i][j] v[j] = (traj[i + 1][j] - traj[i][j]) / dt last_v[j] = (traj[i][j] - traj[i - 1][j]) / dt a[j] = (v[j] - last_v[j]) / dt if i == len(traj) - 1: p = traj[-1] elif i == 0: p = traj[0] point.positions = p point.velocities = v point.accelerations = a point.time_from_start = rospy.Duration((i + 1) * dt) goal.goal.trajectory.points.append(point) self.jta.send_goal_and_wait(goal.goal)
def __extend_trajectory_helper(self, plan, start_pose, travel_distance, step=0.005, time_step=0.1): traj = plan.joint_trajectory.points cur_pose_z = start_pose.position.z start_time = plan.joint_trajectory.points[-1].time_from_start for i in range(0, int(abs(travel_distance) / step)): point = JointTrajectoryPoint() print i joint_goal = self.__inverse_kinematics([start_pose.position.x, start_pose.position.y, cur_pose_z + (np.sign(travel_distance) * step)], [start_pose.orientation.w, start_pose.orientation.x, start_pose.orientation.y, start_pose.orientation.z], NIC_SEED_STATE) if joint_goal == None: print "No IK on extend trajectory found" return None joint_list = [] for idx, jnt in enumerate(joint_goal.tolist()): joint_list.append(jnt) point.positions = joint_list print joint_list point.velocities = [0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.0] point.accelerations = [0,0,0,0,0,0,0] time = rospy.Duration.from_sec(start_time.to_sec() + time_step) start_time += rospy.Duration.from_sec(time_step) point.time_from_start = time traj.append(point) cur_pose_z += np.sign(travel_distance) * step return plan
def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict): if self._mode == 'velocity': velocities = [0.0] * len(joint_names) cmd = dict(zip(joint_names, velocities)) while (not self._server.is_new_goal_available() and self._alive and self.robot_is_enabled()): self._limb.set_joint_velocities(cmd) if self._cuff_state: self._limb.exit_control_mode() break rospy.sleep(1.0 / self._control_rate) elif self._mode == 'position' or self._mode == 'position_w_id': raw_pos_mode = (self._mode == 'position_w_id') if raw_pos_mode: pnt = JointTrajectoryPoint() pnt.positions = self._get_current_position(joint_names) if dimensions_dict['velocities']: pnt.velocities = [0.0] * len(joint_names) if dimensions_dict['accelerations']: pnt.accelerations = [0.0] * len(joint_names) while (not self._server.is_new_goal_available() and self._alive and self.robot_is_enabled()): self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode) # zero inverse dynamics feedforward command if self._mode == 'position_w_id': pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time) ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt) self._pub_ff_cmd.publish(ff_pnt) if self._cuff_state: self._limb.exit_control_mode() break rospy.sleep(1.0 / self._control_rate)
def traj_speed_up(traj, spd=3.0): new_traj = RobotTrajectory() new_traj = traj 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.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd 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] * spd point.accelerations[j] = point.accelerations[j] * spd * spd points[i] = point new_traj.joint_trajectory.points = points return new_traj
def handleJointTrajMsg(data): #### Initialize Speed Parameter # axes [l.x l.y l.z a.x a.y a.z] # scalers = [0.7, 0.7, 0.7, -3.14, -3.14, -3.14] joint_traj_publisher = rospy.Publisher("robotiq_3f_controller/command", JointTrajectory) #### Setup JointTraj Publisher JointTraj = JointTrajectory() JointTrajPoints = JointTrajectoryPoint() #### Start Mapping from PoseStamped to JointTraj JointTraj.header.stamp = rospy.get_rostime() JointTraj.joint_names = ['finger_1_joint_1','finger_1_joint_2','finger_1_joint_3','finger_2_joint_1','finger_2_joint_2','finger_2_joint_3','finger_middle_joint_1','finger_middle_joint_2','finger_middle_joint_3','palm_finger_1_joint','palm_finger_2_joint'] # data size is 30 JointTrajPoints.positions = data.position JointTrajPoints.velocities = data.velocity JointTrajPoints.accelerations = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0] JointTrajPoints.effort = [0] JointTrajPoints.time_from_start = rospy.Duration.from_sec(0.1) JointTraj.points.append(JointTrajPoints) # JointTraj.JointTraj.linear.x=data.pose.position.y*clk*scale_factor_pos*-1 # JointTraj.JointTraj.linear.y=data.pose.position.z*clk*scale_factor_pos # JointTraj.JointTraj.linear.z=data.pose.position.x*clk*scale_factor_pos # JointTraj.JointTraj.angular.x=data.pose.orientation.x # JointTraj.JointTraj.angular.y=data.pose.orientation.y # JointTraj.JointTraj.angular.z=data.pose.orientation.z #### Publish msg rate = rospy.Rate(100) # 100hz rospy.loginfo(data) joint_traj_publisher.publish(JointTraj) rate.sleep()
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 make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id="/base_link"): """ This function will convert the plan to a joint trajectory compatible with the /arm_N/arm_controller/command topic """ theplan = plan # create joint trajectory message jt = JointTrajectory() # fill the header jt.header.seq = seq jt.header.stamp.secs = 0 # secs jt.header.stamp.nsecs = 0 # nsecs jt.header.frame_id = frame_id # specify the joint names jt.joint_names = theplan.joint_trajectory.joint_names # fill the trajectory points and computer constraint times njtp = len(theplan.joint_trajectory.points) for ii in range(0, njtp): jtp = JointTrajectoryPoint() jtp = copy.deepcopy(theplan.joint_trajectory.points[ii]) jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0] jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0] jtp.time_from_start = rospy.Duration.from_sec(secs + dt * (ii + 1)) jt.points.append(jtp) return jt
def ProcessTrajectory(self, trajectory, pres_joint_pos, string_trajectories=False): joint_names = trajectory.joint_names num_joints = len(joint_names) trajectory_points = trajectory.points pnt_times = [0.0] * len(trajectory_points) dimensions_dict = self._determine_dimensions(trajectory_points) # Create a new discretized joint trajectory num_points = len(trajectory_points) if num_points == 0: rospy.logerr("%s: Empty Trajectory" % (action_name, )) return False, [] if num_points == 1: # Add current position as trajectory point first_trajectory_point = JointTrajectoryPoint() first_trajectory_point.positions = [ pres_joint_pos[jnt] for jnt in joint_names ] # To preserve desired velocities and accelerations, copy them to the first # trajectory point if the trajectory is only 1 point. if dimensions_dict['velocities']: first_trajectory_point.velocities = deepcopy( trajectory_points[0].velocities) if dimensions_dict['accelerations']: first_trajectory_point.accelerations = deepcopy( trajectory_points[0].accelerations) first_trajectory_point.time_from_start = rospy.Duration(0) trajectory_points.insert(0, first_trajectory_point) num_points = len(trajectory_points) # Force Velocites/Accelerations to zero at the final timestep # if they exist in the trajectory # Remove this behavior if you are stringing together trajectories, # and want continuous, non-zero velocities/accelerations between # trajectories if not string_trajectories: if dimensions_dict['velocities']: trajectory_points[-1].velocities = [0.0] * len(joint_names) if dimensions_dict['accelerations']: trajectory_points[-1].accelerations = [0.0] * len(joint_names) # Compute Full Bezier Curve Coefficients for all 7 joints pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points] try: b_matrix = self._compute_bezier_coeff(joint_names, trajectory_points, dimensions_dict) except Exception as ex: rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}" " arm with error \"{2}: {3}\"").format( self._action_name, self._name, type(ex).__name__, ex)) return False, [] return True, [ dimensions_dict, b_matrix, trajectory_points, pnt_times, num_points ]
def cartesian_path2joint_path(waypoints, robot): trajectory = JointTrajectory() trajectory.joint_names = robot.joint_names('left_arm') time_sec = 0.5 time_incr = 0.5 for i in range(len(waypoints)): wpt = waypoints[i] joint_positions = robot.inverse_kinematics( [wpt.position.x, wpt.position.y, wpt.position.z], [ wpt.orientation.x, wpt.orientation.y, wpt.orientation.z, wpt.orientation.w ]) if joint_positions is not None: point = JointTrajectoryPoint() point.positions = joint_positions point.velocities = [0.0] * len(joint_positions) point.accelerations = [0.0] * len(joint_positions) point.time_from_start = rospy.Duration(time_sec) trajectory.points.append(point) else: rospy.logwarn("Failed to find ik solution for grasp waypoint %d" % (i, )) time_sec += time_incr return trajectory
def followTrajectory(self, traj): # Generate ROS trajectory message # Don't send head pose! plan_runner freaks out msg = SendJointTrajectoryRequest() msg.trajectory.header.stamp = rospy.Time.now() msg.trajectory.header.seq = 0 msg.trajectory.joint_names = self.joint_names[2:] for sol in traj: # print "sol:", sol point = JointTrajectoryPoint() point.time_from_start = rospy.Duration(sol[0]) point.positions = sol[3:] point.velocities = [0.0]*len(point.positions) point.accelerations = [0.0]*len(point.positions) point.effort = [0.0]*len(point.positions) msg.trajectory.points.append(point) # Generate ROS head message # All the head poses are the same -- just use the last one # print "Moving head to pose:", traj[-1, 1:3] head_msg = MoveHeadRequest(traj[-1, 1:3]) self.move_head_service(head_msg) # Call SendTrajectory service self.traj_service(msg)
def handle_joints_move(req): goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = joint_names current_joints = get_current_joints() max_inc = max( map(lambda (x, y): abs(x - y), zip(req.positions, current_joints))) if abs(max_inc) < 0.0174: return QueryJointsMovementResponse(success=1) j_traj = JointTrajectoryPoint() #duration = max_inc/req.velocity duration = 2 j_traj.time_from_start = rospy.Duration(duration) j_traj.positions = req.positions j_traj.velocities = [0] * 7 j_traj.accelerations = [0] * 7 j_traj.effort = [0.0] * 7 goal.trajectory.points.append(j_traj) try: # Creates the SimpleActionClient, passing the type of the action # (FollowJointTrajectoryAction) to the constructor. action_client = actionlib.SimpleActionClient( action_client_name, FollowJointTrajectoryAction) # Waits until the action server has started up and started # listening for goals. action_client.wait_for_server() # Send goal action_client.send_goal(goal) rospy.sleep(duration) except rospy.ROSInterruptException as e: print(str(e)) return QueryJointsMovementResponse(success=1)
def move_arm(self, points_list): # # Unpause the physics # rospy.wait_for_service('/gazebo/unpause_physics') # unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) # unpause_gazebo() self.client.wait_for_server() goal = FollowJointTrajectoryGoal() # We need to fill the goal message with its components # # check msg structure with: rosmsg info FollowJointTrajectoryGoal # It is composed of 4 sub-messages: # "trajectory" of type trajectory_msgs/JointTrajectory # "path_tolerance" of type control_msgs/JointTolerance # "goal_tolerance" of type control_msgs/JointTolerance # "goal_time_tolerance" of type duration trajectory_msg = JointTrajectory() # check msg structure with: rosmsg info JointTrajectory # It is composed of 3 sub-messages: # "header" of type std_msgs/Header # "joint_names" of type string # "points" of type trajectory_msgs/JointTrajectoryPoint trajectory_msg.joint_names = [ "j2n6s300_joint_1", "j2n6s300_joint_2", "j2n6s300_joint_3", "j2n6s300_joint_4", "j2n6s300_joint_5", "j2n6s300_joint_6" ] points_msg = JointTrajectoryPoint() # check msg structure with: rosmsg info JointTrajectoryPoint # It is composed of 5 sub-messages: # "positions" of type float64 # "velocities" of type float64 # "accelerations" of type float64 # "efforts" of type float64 # "time_from_start" of type duration points_msg.positions = [0, 0, 0, 0, 0, 0] points_msg.velocities = [0, 0, 0, 0, 0, 0] points_msg.accelerations = [0, 0, 0, 0, 0, 0] points_msg.effort = [0, 1, 0, 0, 0, 0] points_msg.time_from_start = rospy.Duration(0.01) # fill in points message of the trajectory message trajectory_msg.points = [points_msg] # fill in trajectory message of the goal goal.trajectory = trajectory_msg print(trajectory_msg) # self.client.send_goal_and_wait(goal) self.client.send_goal(goal) self.client.wait_for_result() rospy.sleep(2) # wait for 2s
def add_point(self, positions, time): point = JointTrajectoryPoint() point.positions = copy(positions) point.velocities = copy(np.zeros(len(positions))) point.accelerations = copy(np.zeros(len(positions))) point.effort = copy(np.zeros(len(positions))) point.time_from_start = rospy.Duration(time) self._goal.trajectory.points.append(point)
def talker(): if 1 == 0: pub = rospy.Publisher("/ihmc_ros/atlas/control/arm_joint_trajectory2", JointTrajectory, queue_size=10) jn = ["l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx", "l_arm_wry2"] jn_r = ["r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx", "r_arm_wry2"] else: pub = rospy.Publisher("/ihmc_ros/valkyrie/control/arm_joint_trajectory2", JointTrajectory, queue_size=10) jn = [ "LeftShoulderPitch", "LeftShoulderRoll", "LeftShoulderYaw", "LeftElbowPitch", "LeftForearmYaw", "LeftWristRoll", "LeftWristPitch", ] jn_r = [ "RightShoulderPitch", "RightShoulderRoll", "RightShoulderYaw", "RightElbowPitch", "RightForearmYaw", "RightWristRoll", "RightWristPitch", ] # this doesnt work: # jn = ["l_arm_shz","l_arm_shx","l_arm_ely","l_arm_elx","l_arm_wry","l_arm_wrx","l_arm_wry2", "r_arm_shz","l_arm_shx","r_arm_ely","r_arm_elx","r_arm_wry","r_arm_wrx","r_arm_wry2"] # value = 0 rospy.init_node("send_arm_test", anonymous=True) rate = rospy.Rate(1) # 10hz while not rospy.is_shutdown(): msg = JointTrajectory() value = 0.1 value_r = 0.1 msg.joint_names = jn pt = JointTrajectoryPoint() pt.positions = [value] * len(jn) pt.velocities = [0] * len(jn) pt.accelerations = [0] * len(jn) pt.effort = [0] * len(jn) pt.time_from_start = rospy.Duration.from_sec(3) msg.points.append(pt) print msg.joint_names, rospy.get_time() pub.publish(msg) # TODO: add a sleep here between left and right msg.joint_names = jn_r msg.points[0].positions = [value_r] * len(jn) print msg.joint_names, rospy.get_time() pub.publish(msg) rate.sleep()
def construct_joint_point(position, t): point = JointTrajectoryPoint() point.positions = position if not isinstance(t, rospy.Duration): t = rospy.Duration(t) point.velocities = [0.0] * len(position) point.accelerations = [0.0] * len(position) point.time_from_start = t return point
def trajectory_point(self, t, jointspace): """ takes a discrete point in time, and puts the position, velocity, and acceleration into a ROS JointTrajectoryPoint() to be put into a RobotTrajectory. Parameters ---------- t : float 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. Returns ------- :obj:`trajectory_msgs.msg.JointTrajectoryPoint` """ point = JointTrajectoryPoint() delta_t = .01 if jointspace: x_t, x_t_1, x_t_2 = None, None, None ik_attempts = 0 theta_t_2 = self.get_ik(self.target_position(t-2*delta_t)) theta_t_1 = self.get_ik(self.target_position(t-delta_t)) theta_t = self.get_ik(self.target_position(t)) # print(self.target_position(t)) #theta_t = np.array(theta_t) # print(theta_t) # we said you shouldn't simply take a finite difference when creating # the path, why do you think we're doing that here? cause you're mean point.positions = theta_t point.velocities = (theta_t - theta_t_1) / delta_t point.accelerations = (theta_t - 2*theta_t_1 + theta_t_2) / (2*delta_t) else: point.positions = self.target_position(t) point.velocities = self.target_velocity(t) point.accelerations = self.target_acceleration(t) point.time_from_start = rospy.Duration.from_sec(t) return point
def set_position(self,goal): traj = JointTrajectory() traj_p = JointTrajectoryPoint() traj.joint_names = ["torso_lift_joint"] traj_p.positions = [goal] traj_p.velocities = [0.] traj_p.accelerations = [0.] traj_p.time_from_start = rospy.Duration(2.) traj.points.append(traj_p) self.pub.publish(traj)
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 make_joint_trajectory(names, points): jt = JointTrajectory() jt.joint_names = names pt = JointTrajectoryPoint() pt.positions = points pt.effort = [0]*len(points) pt.velocities = [0]*len(points) pt.accelerations = [0]*len(points) jt.points = [pt] return jt
def point(): global joy_r_v_avg global joy_r_h_avg global commanded_elev global commanded_azim rospy.init_node('manual_arm_control') rate = rospy.Rate(50) pub = rospy.Publisher('/arm_twist', JointTrajectoryPoint, queue_size=1) rospy.Subscriber('/arm_start_stop', Bool, run_cb) rospy.Subscriber("/joystick", Joy, joy_cb) MAX_AZIM = rospy.get_param("/max_azimuth", np.pi * 2) MIN_AZIM = rospy.get_param("/min_azimuth", -np.pi * 2) MAX_ELEV = rospy.get_param("/max_elevation", np.pi / 4) MIN_ELEV = rospy.get_param("/min_elevation", -np.pi / 4) AZIM_MAX_SPEED = rospy.get_param("/max_azimuth_speed", np.pi / 2) ELEV_MAX_SPEED = rospy.get_param("/max_elev_speed", np.pi / 16) while not rospy.is_shutdown(): if run_var == True: commanded_elev_speed = joy_r_v_avg * ELEV_MAX_SPEED commanded_elev += commanded_elev_speed * dt commanded_azim_speed = joy_r_h_avg * AZIM_MAX_SPEED if commanded_azim_speed > 0: commanded_azim = 100 else: commanded_azim = -100 if commanded_azim > MAX_AZIM: commanded_azim = MAX_AZIM #commanded_azim_speed = 0 elif commanded_azim < MIN_AZIM: commanded_azim = MIN_AZIM #commanded_azim_speed = 0 if commanded_elev > MAX_ELEV: commanded_elev = MAX_ELEV #commanded_elev_speed = 0 elif commanded_elev < MIN_ELEV: commanded_elev = MIN_ELEV #commanded_elev_speed = 0 p = JointTrajectoryPoint() azim = commanded_azim elev = commanded_elev azim_speed = commanded_azim_speed elev_speed = commanded_elev_speed p.positions = [commanded_azim, commanded_elev] p.velocities = [commanded_azim_speed * 8, commanded_elev_speed * 2] p.accelerations = [0, 0] #p.time_from_start = rospy.Time.now() pub.publish(p) rate.sleep() '''elif run_var == False:''' #TODO Set zero to current joint positions to avoid jumps
def execute_cb(self, goal): arm_goal = FollowJointTrajectoryGoal() arm_goal.goal_time_tolerance = rospy.Time(1.0) arm_goal.trajectory.joint_names = [ 'arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4', 'arm_joint_5' ] for pt in goal.trajectory.points: pt_temp = JointTrajectoryPoint() pt_temp.positions = pt.positions[0:5] pt_temp.velocities = pt.velocities[0:5] pt_temp.accelerations = pt.accelerations[0:5] pt_temp.time_from_start = pt.time_from_start arm_goal.trajectory.points.append(pt_temp) base_goal = FollowJointTrajectoryGoal() base_goal.goal_time_tolerance = rospy.Time(1.0) base_goal.trajectory.joint_names = [ 'virtual_x', 'virtual_y', 'virtual_theta' ] for pt in goal.trajectory.points: pt_temp = JointTrajectoryPoint() pt_temp.positions.append(pt.positions[6]) pt_temp.positions.append(pt.positions[7]) pt_temp.positions.append(pt.positions[5]) pt_temp.velocities.append(pt.velocities[6]) pt_temp.velocities.append(pt.velocities[7]) pt_temp.velocities.append(pt.velocities[5]) pt_temp.accelerations.append(pt.accelerations[6]) pt_temp.accelerations.append(pt.accelerations[7]) pt_temp.accelerations.append(pt.accelerations[5]) pt_temp.time_from_start = pt.time_from_start base_goal.trajectory.points.append(pt_temp) #rospy.loginfo(base_goal.trajectory.points[0].positions) #list of positions of one point #rospy.loginfo(base_goal.trajectory.points[1].positions) #rospy.loginfo(base_goal.trajectory.points[2].positions) if self._as.is_preempt_requested(): rospy.loginfo("This action has been preempted") self._as.set_preempted() success = False self._as.set_cancelled() else: #rospy.loginfo(base_goal.trajectory.points) self.base_client.send_goal(base_goal) self.arm_client.send_goal_and_wait(arm_goal) success = True if success: rospy.loginfo('Action Succeeded') self._as.set_succeeded()
def move_jtp(self, pos): jtp_msg = JointTrajectory() jtp_msg.joint_names = self.joint_name_lst point = JointTrajectoryPoint() point.positions = pos point.velocities = self.jtp_zeros point.accelerations = self.jtp_zeros point.effort = self.jtp_zeros point.time_from_start = rospy.Duration(1.0 / 60.0) jtp_msg.points.append(point) self.jtp.publish(jtp_msg)
def create_JTG(self, arm, pos_arr, dur_arr, stamp=None, vel_arr=None, acc_arr=None): # Hack vel_arr = [[0.]*7]*len(pos_arr) acc_arr = [[0.]*7]*len(pos_arr) ## # Compute joint velocities and acclereations. def get_vel_acc(q_arr, d_arr): vel_arr = [[0.]*7] acc_arr = [[0.]*7] for i in range(1, len(q_arr)): vel, acc = [], [] for j in range(7): vel += [(q_arr[i][j] - q_arr[i-1][j]) / d_arr[i]] acc += [(vel[j] - vel_arr[i-1][j]) / d_arr[i]] vel_arr += [vel] acc_arr += [acc] print vel, acc return vel_arr, acc_arr if arm != 1: arm = 0 jtg = JointTrajectoryGoal() if stamp is None: stamp = rospy.Time.now() else: jtg.trajectory.header.stamp = stamp if len(pos_arr) > 1 and (vel_arr is None or acc_arr is None): v_arr, a_arr = get_vel_acc(pos_arr, dur_arr) if vel_arr is None: vel_arr = v_arr if acc_arr is None: acc_arr = a_arr jtg.trajectory.joint_names = self.joint_names_list[arm] for i in range(len(pos_arr)): if pos_arr[i] is None or type(pos_arr[i]) is types.NoneType: continue jtp = JointTrajectoryPoint() jtp.positions = pos_arr[i] if vel_arr is None: vel = [0.] * 7 else: vel = vel_arr[i] jtp.velocities = vel if acc_arr is None: acc = [0.] * 7 else: acc = acc_arr[i] jtp.accelerations = acc jtp.time_from_start = rospy.Duration(dur_arr[i]) jtg.trajectory.points.append(jtp) return jtg
def cmd_pub(self): point_goal = JointTrajectoryPoint() point_goal.positions = [0.4, -0.5] point_goal.velocities = [0, 0] point_goal.accelerations = [0, 0] point_goal.time_from_start = rospy.Time(secs=2, nsecs=0) self.action_goal.trajectory.points.append(point_goal) self.action_client.wait_for_server() self.action_client.send_goal(self.action_goal) self.action_goal.trajectory.points = []
def jointTrajectoryPointFromJointState(jointState, timeFromStart): trajPoint = JointTrajectoryPoint() trajPoint.positions = jointState.position numJoints = len(jointState.position) trajPoint.velocities = [0] * numJoints trajPoint.accelerations = [0] * numJoints trajPoint.effort = [0] * numJoints trajPoint.time_from_start = timeFromStart return trajPoint
def createArmTrajectory(self, values): traj_msg = JointTrajectory() traj_msg.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0) traj_msg.joint_names = self.GetJointNames() traj_msg.points = [] end_point = JointTrajectoryPoint() end_point.positions = values end_point.velocities = [0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] end_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] end_point.time_from_start = rospy.Duration.from_sec(1.0) traj_msg.points.append(end_point) return traj_msg
def stop(self, include_velocity=False, include_effort=False): """ Stop the recording and returns the recorded trajectory of joints declared in the constructor :param include_velocity: if True, the joint velocities will be included in the returned trajectory :param include_effort: if True, the joint efforts will be included in the returned trajectory :return: (joint_traj, eef_traj) resp. the recorded joint trajectory (RobotTrajectory) and end effector trajectory in frame 'base' (Path) """ with self._recording_lock: self._recording = False # Now we reconstruct a RobotTrajectory... rt = RobotTrajectory() rt.joint_trajectory.joint_names = self._joints with self._joint_recorder_lock: for idx, js in enumerate(self._joint_recorded_points): jtp = JointTrajectoryPoint() try: jtp.positions = [ js.position[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names ] except ValueError as e: # This specific JS sample does not contain the desired joints, skip it continue else: if include_velocity: jtp.velocities = [ js.velocity[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names ] if include_effort: jtp.accelerations = [ js.effort[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names ] jtp.time_from_start = self._joint_recorded_times[ idx] - self._joint_recorded_times[0] rt.joint_trajectory.points.append(jtp) # ... as well as an End Effector trajectory eft = Path() with self._eef_recorder_lock: for eef in self._eef_recorded_points: ps = PoseStamped() ps.header = eef.header ps.header.frame_id = 'base' ps.pose = eef.pose eft.poses.append(ps) # twist and wrench in eef are discarded eft.header.frame_id = 'base' return rt, eft
def send_left_arm_goal(self, j_positions): jt = JointTrajectory() jt.header.stamp = rospy.Time.now() jt.joint_names = ['l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] jtp = JointTrajectoryPoint() jtp.positions = list(j_positions) jtp.velocities = [0.0] * len(j_positions) jtp.accelerations = [0.0] * len(j_positions) jtp.time_from_start = rospy.Duration(0.45) jt.points.append(jtp) self.left_command.publish(jt)
def trapezoid_gen(target, current_joint_angles, duration): goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] goal.trajectory.header.frame_id = '/world' dist = target - current_joint_angles vmax = 1.5 * dist / duration acc = 3 * vmax / duration p1 = JointTrajectoryPoint() p1.positions = current_joint_angles p1.velocities = np.zeros((6, )) p1.accelerations = acc p1.time_from_start = rospy.Duration(0) p2 = JointTrajectoryPoint() p2.positions = np.array(p1.positions) + dist / 4 p2.velocities = vmax p2.accelerations = np.zeros((6, )) p2.time_from_start = rospy.Duration(duration / 3) p3 = JointTrajectoryPoint() p3.positions = np.array(p1.positions) + 3 * dist / 4 p3.velocities = vmax p3.accelerations = -acc p3.time_from_start = rospy.Duration(duration * 2 / 3) p4 = JointTrajectoryPoint() p4.positions = target p4.velocities = np.zeros((6, )) p4.accelerations = np.zeros((6, )) p4.time_from_start = rospy.Duration(duration) goal.trajectory.points.append(p1) goal.trajectory.points.append(p2) goal.trajectory.points.append(p3) goal.trajectory.points.append(p4) return goal
def set_jointangles(self, arm, q, duration=0.15): rospy.logwarn('Currently ignoring the arm parameter.') # for i,p in enumerate(self.r_arm_pub_l): # p.publish(q[i]) jtg = JointTrajectoryGoal() jtg.trajectory.joint_names = self.joint_names_list jtp = JointTrajectoryPoint() jtp.positions = q jtp.velocities = [0 for i in range(len(q))] jtp.accelerations = [0 for i in range(len(q))] jtp.time_from_start = rospy.Duration(duration) jtg.trajectory.points.append(jtp) self.joint_action_client.send_goal(jtg)
def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict): if not self._alive: self._limb.exit_control_mode() elif self._mode == 'joint_impedance': pnt = JointTrajectoryPoint() pnt.positions = self._get_current_position(joint_names) if self._mode == 'joint_impedance' and self._alive: # zero inverse dynamics feedforward command pnt.velocities = [0.0] * len(joint_names) pnt.accelerations = [0.0] * len(joint_names) self._limb.cmd_joint_angles(pnt.positions, pnt.velocities, pnt.accelerations)
def position_callback(self, msg): action_goal = FollowJointTrajectoryActionGoal() action_goal.goal_id.stamp = rospy.get_rostime() action_goal.goal.trajectory.joint_names = self._joint_names target_point = JointTrajectoryPoint() if self._dof == len(msg.positions): target_point.positions = msg.positions else: target_point.positions = [] for position in msg.positions: target_point.positions.append(position) for i in range(self._dof - len(msg.positions)): target_point.positions.append(0.0) target_point.velocities = [self._velocity] * self._dof target_point.accelerations = [self._acceleration] * self._dof action_goal.goal.trajectory.points.append(target_point) self._pub.publish(action_goal)
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 createGoalWithValueAndPublish(self, value): jt = JointTrajectory() jt.joint_names = self.joint_names jtp = JointTrajectoryPoint() # Get current values and modify the one we want names, values = self.getNamesAndMsgListAndModifyValue(self.joint_names, self.joint_name, value) jtp.positions = values jtp.velocities = [0.0] * len(self.joint_names) jtp.accelerations = [0.0] * len(self.joint_names) # TODO: Make duration dependent on how big is the movement scaled_time = abs(value) * 1.5 + 0.2 jtp.time_from_start = rospy.Duration(scaled_time) jt.points.append(jtp) self.pub.publish(jt) rospy.sleep(1.0)
def receiverLoop(self): while True: try: input_msg = pickle.load( open( "/dev/shm/right_arm_goal.p", "rb" ) ) os.remove("/dev/shm/right_arm_goal.p") except IOError: print "file not there..." rospy.sleep(0.1) continue print input_msg goal = FollowJointTrajectoryGoal() rospy.loginfo("input_msg.goal.goal_time_tolerance: \n" + str(input_msg.goal.goal_time_tolerance ) ) goal.goal_time_tolerance = rospy.Duration(secs=0, nsecs=0) rospy.loginfo("input_msg.goal.goal_tolerance: \n" + str(input_msg.goal.goal_tolerance ) ) goal.goal_tolerance = input_msg.goal.goal_tolerance rospy.loginfo("input_msg.goal.path_tolerance: \n" + str(input_msg.goal.path_tolerance ) ) goal.path_tolerance = input_msg.goal.path_tolerance rospy.loginfo("input_msg.goal.trajectory.header: \n" + str(input_msg.goal.trajectory.header ) ) goal.trajectory.header = input_msg.goal.trajectory.header goal.trajectory.header.frame_id = '/base_link' goal.trajectory.joint_names = input_msg.goal.trajectory.joint_names input_times = pickle.load( open( "/dev/shm/right_arm_goal_times.p", "rb" ) ) rospy.loginfo("input_times is:\n" + str(input_times)) for input_point, time in zip(input_msg.goal.trajectory.points, input_times): p = JointTrajectoryPoint() p.accelerations = input_point.accelerations p.positions = input_point.positions p.velocities = input_point.velocities # rospy.loginfo("???????????????????????????????????????????????????????????????") # rospy.loginfo("Time from start is:\n" + str(input_point.time_from_start)) # rospy.loginfo("time is:\n" + str(time)) # rospy.loginfo("???????????????????????????????????????????????????????????????") p.time_from_start = time goal.trajectory.points.append(p) os.remove("/dev/shm/right_arm_goal_times.p") #print "Goal will be:" rospy.loginfo("!!!!!!!!!!!Goal:\n" +str(goal)) self.right_arm_as.send_goal(goal) rospy.loginfo("Waiting for result") self.right_arm_as.wait_for_result() rospy.loginfo("Goal done")
def move_to(self, positions, velocities, accelerations, delta_t): assert(len(self.joint_names) == positions.shape[1]) trajectory = JointTrajectory() trajectory.joint_names = self.joint_names for i in range(positions.shape[0]): # for each row i p = JointTrajectoryPoint() p.positions = list(positions[i]) p.velocities = list(velocities[i]) p.accelerations = list(accelerations[i]) p.time_from_start = rospy.Duration(i * delta_t) trajectory.points.append(p) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self.client.send_goal(follow_goal) self.client.wait_for_result()
def stop(self, include_velocity=False, include_effort=False): """ Stop the recording and returns the recorded trajectory of joints declared in the constructor :param include_velocity: if True, the joint velocities will be included in the returned trajectory :param include_effort: if True, the joint efforts will be included in the returned trajectory :return: (joint_traj, eef_traj) resp. the recorded joint trajectory (RobotTrajectory) and end effector trajectory in frame 'base' (Path) """ with self._recording_lock: self._recording = False # Now we reconstruct a RobotTrajectory... rt = RobotTrajectory() rt.joint_trajectory.joint_names = self._joints with self._joint_recorder_lock: for idx, js in enumerate(self._joint_recorded_points): jtp = JointTrajectoryPoint() try: jtp.positions = [js.position[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names] except ValueError as e: # This specific JS sample does not contain the desired joints, skip it continue else: if include_velocity: jtp.velocities = [js.velocity[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names] if include_effort: jtp.accelerations = [js.effort[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names] jtp.time_from_start = self._joint_recorded_times[idx] - self._joint_recorded_times[0] rt.joint_trajectory.points.append(jtp) # ... as well as an End Effector trajectory eft = Path() with self._eef_recorder_lock: for eef in self._eef_recorded_points: ps = PoseStamped() ps.header = eef.header ps.header.frame_id = 'base' ps.pose = eef.pose eft.poses.append(ps) # twist and wrench in eef are discarded eft.header.frame_id = 'base' return rt, eft
def track_trajectory( pos, vel, acc, time, arm = 'r', client = None, stamp = None): if (acc == None): acc = np.zeros( pos.shape ) if ( len( pos ) != len( vel ) or len( pos ) != len( time ) ): rospy.logerr( '[track_trajectory] dimensions do not agree' ) rospy.loginfo( 'arm \'%s\' tracking trajectory with %d points' %( arm, len( pos ) ) ) if (client == None): client = init_jt_client(arm) else: arm = client.action_client.ns[0:1]; # ignore arm argument joint_names = get_joint_names( ''.join( ( arm, '_arm_controller' ) ) ) # create trajectory message jt = JointTrajectory() jt.joint_names = joint_names jt.points = [] t = 0 for i in range( len( pos ) ): jp = JointTrajectoryPoint() jp.time_from_start = rospy.Time.from_sec( time[i] ) jp.positions = pos[i,:] jp.velocities = vel[i,:] jp.accelerations = acc[i,:] jt.points.append( jp ) t += 1 if ( stamp == None ): stamp = rospy.Time.now() # push trajectory goal to ActionServer jt_goal = JointTrajectoryGoal() jt_goal.trajectory = jt jt_goal.trajectory.header.stamp = stamp; client.send_goal(jt_goal)
def spam_controller(controller_name, joint_names): cmd_topic = "/" + controller_name + "/command" rospy.loginfo("Going to spam controller: " + controller_name + "\n in command topic: " + cmd_topic + "\n with joints: " + str(joint_names) ) ctl_pub = rospy.Publisher(cmd_topic, JointTrajectory) rospy.sleep(0.1) while not rospy.is_shutdown(): jt = JointTrajectory() jt.joint_names = joint_names jtp = JointTrajectoryPoint() jtp.positions = get_valid_random_values(joint_names, joints_dict) jtp.velocities = [0.0] * len(joint_names) jtp.accelerations = [0.0] * len(joint_names) jtp.time_from_start = rospy.Duration(random.random() + 0.5) # 0.5s to 1.5s jt.points.append(jtp) ctl_pub.publish(jt) rospy.logdebug("Sent to: " + cmd_topic + "\n" + str(jt)) rospy.sleep(jtp.time_from_start)
def _get_bezier_point(self, b_matrix, idx, t, cmd_time, dimensions_dict): pnt = JointTrajectoryPoint() pnt.time_from_start = rospy.Duration(cmd_time) num_joints = b_matrix.shape[0] pnt.positions = [0.0] * num_joints if dimensions_dict["velocities"]: pnt.velocities = [0.0] * num_joints if dimensions_dict["accelerations"]: pnt.accelerations = [0.0] * num_joints for jnt in range(num_joints): b_point = bezier.bezier_point(b_matrix[jnt, :, :, :], idx, t) # Positions at specified time pnt.positions[jnt] = b_point[0] # Velocities at specified time if dimensions_dict["velocities"]: pnt.velocities[jnt] = b_point[1] # Accelerations at specified time if dimensions_dict["accelerations"]: pnt.accelerations[jnt] = b_point[-1] return pnt
def _get_point(self, joint_names, time): pnt = JointTrajectoryPoint() pnt.time_from_start = rospy.Duration(time) pnt.positions = [0.0] * len(joint_names) pnt.velocities = [0.0] * len(joint_names) pnt.accelerations = [0.0] * len(joint_names) time2 = math.pow(time, 2) time3 = math.pow(time, 3) time4 = math.pow(time, 4) time5 = math.pow(time, 5) for jnt in xrange(len(joint_names)): # Positions at specified time pnt.positions[jnt] = (self._coeff[jnt][0] + time * self._coeff[jnt][1] + time2 * self._coeff[jnt][2] + time3 * self._coeff[jnt][3] + time4 * self._coeff[jnt][4] + time5 * self._coeff[jnt][5] ) # Velocities at specified time pnt.velocities[jnt] = (self._coeff[jnt][1] + 2.0 * time * self._coeff[jnt][2] + (3.0 * time2 * self._coeff[jnt][3]) + (4.0 * time3 * self._coeff[jnt][4]) + (5.0 * time4 * self._coeff[jnt][5]) ) # Accelerations at specified time pnt.accelerations[jnt] = (2.0 * self._coeff[jnt][2] + 6.0 * time * self._coeff[jnt][3] + (12.0 * time2 * self._coeff[jnt][4]) + (20.0 * time3 * self._coeff[jnt][5]) ) return pnt
def receiverLoop(self): while True: print "self.conn.recv()" input_msg = self.conn.recv() print input_msg goal = FollowJointTrajectoryGoal() goal.goal_time_tolerance = input_msg.goal_time_tolerance goal.goal_tolerance = input_msg.goal_tolerance goal.path_tolerance = input_msg.path_tolerance goal.trajectory.header = input_msg.trajectory.header goal.trajectory.joint_names = input_msg.joint_names for input_point in input_msg.trajectory.points: p = JointTrajectoryPoint() p.accelerations = input_point.accelerations p.positions = input_point.positions p.velocities = input_point.velocities p.time_from_start = input_point.time_from_start goal.trajectory.points.append(p) print "Goal will be:" print goal #self.right_arm_as.send_goal(goal) rospy.loginfo("Waiting for result") self.right_arm_as.wait_for_result() rospy.loginfo("Goal done")
pos = pos.split(", ") for i in range(0, len(pos)): pos[i] = float(pos[i]) y.positions = pos #get vel vel = el[counter + 2].split(": ")[1][1:-1] vel = vel.split(", ") for i in range(0, len(vel)): vel[i] = float(vel[i]) y.velocities = vel #get acc acc = el[counter + 3].split(": ")[1][1:-1] acc = acc.split(", ") for i in range(0, len(acc)): acc[i] = float(acc[i]) y.accelerations = acc #get time st = el[counter + 6].split(": ")[1] et = el[counter + 7].split(": ")[1] y.time_from_start.secs = float(st) y.time_from_start.nsecs = float(et) x.joint_trajectory.points.append(y) counter = counter + 8 acHan.execute_plan(x) t.close()
def _on_trajectory_action(self, goal): joint_names = goal.trajectory.joint_names trajectory_points = goal.trajectory.points # Load parameters for trajectory self._get_trajectory_parameters(joint_names, goal) # Create a new discretized joint trajectory num_points = len(trajectory_points) if num_points == 0: rospy.logerr("%s: Empty Trajectory" % (self._action_name,)) self._server.set_aborted() return rospy.loginfo("%s: Executing requested joint trajectory" % (self._action_name,)) rospy.logdebug("Trajectory Points: {0}".format(trajectory_points)) control_rate = rospy.Rate(self._control_rate) dimensions_dict = self._determine_dimensions(trajectory_points) if num_points == 1: # Add current position as trajectory point first_trajectory_point = JointTrajectoryPoint() first_trajectory_point.positions = self._get_current_position(joint_names) # To preserve desired velocities and accelerations, copy them to the first # trajectory point if the trajectory is only 1 point. if dimensions_dict["velocities"]: first_trajectory_point.velocities = deepcopy(trajectory_points[0].velocities) if dimensions_dict["accelerations"]: first_trajectory_point.accelerations = deepcopy(trajectory_points[0].accelerations) first_trajectory_point.time_from_start = rospy.Duration(0) trajectory_points.insert(0, first_trajectory_point) num_points = len(trajectory_points) # Force Velocites/Accelerations to zero at the final timestep # if they exist in the trajectory # Remove this behavior if you are stringing together trajectories, # and want continuous, non-zero velocities/accelerations between # trajectories if dimensions_dict["velocities"]: trajectory_points[-1].velocities = [0.0] * len(joint_names) if dimensions_dict["accelerations"]: trajectory_points[-1].accelerations = [0.0] * len(joint_names) # Compute Full Bezier Curve Coefficients for all 7 joints pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points] try: b_matrix = self._compute_bezier_coeff(joint_names, trajectory_points, dimensions_dict) except Exception as ex: rospy.logerr( ("{0}: Failed to compute a Bezier trajectory for {1}" ' arm with error "{2}: {3}"').format( self._action_name, self._name, type(ex).__name__, ex ) ) self._server.set_aborted() return # Wait for the specified execution time, if not provided use now start_time = goal.trajectory.header.stamp.to_sec() if start_time == 0.0: start_time = rospy.get_time() baxter_dataflow.wait_for(lambda: rospy.get_time() >= start_time, timeout=float("inf")) # Loop until end of trajectory time. Provide a single time step # of the control rate past the end to ensure we get to the end. # Keep track of current indices for spline segment generation now_from_start = rospy.get_time() - start_time end_time = trajectory_points[-1].time_from_start.to_sec() while now_from_start < end_time and not rospy.is_shutdown(): # Acquire Mutex now = rospy.get_time() now_from_start = now - start_time idx = bisect.bisect(pnt_times, now_from_start) # Calculate percentage of time passed in this interval if idx >= num_points: cmd_time = now_from_start - pnt_times[-1] t = 1.0 elif idx >= 0: cmd_time = now_from_start - pnt_times[idx - 1] t = cmd_time / (pnt_times[idx] - pnt_times[idx - 1]) else: cmd_time = 0 t = 0 point = self._get_bezier_point(b_matrix, idx, t, cmd_time, dimensions_dict) # Command Joint Position, Velocity, Acceleration command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict) self._update_feedback(deepcopy(point), joint_names, now_from_start) # Release the Mutex if not command_executed: return control_rate.sleep() # Keep trying to meet goal until goal_time constraint expired last = trajectory_points[-1] last_time = trajectory_points[-1].time_from_start.to_sec() end_angles = dict(zip(joint_names, last.positions)) def check_goal_state(): for error in self._get_current_error(joint_names, last.positions): if self._goal_error[error[0]] > 0 and self._goal_error[error[0]] < math.fabs(error[1]): return error[0] if ( self._stopped_velocity > 0.0 and max([abs(cur_vel) for cur_vel in self._get_current_velocities(joint_names)]) > self._stopped_velocity ): return False else: return True while now_from_start < (last_time + self._goal_time) and not rospy.is_shutdown(): if not self._command_joints(joint_names, last, start_time, dimensions_dict): return now_from_start = rospy.get_time() - start_time self._update_feedback(deepcopy(last), joint_names, now_from_start) control_rate.sleep() now_from_start = rospy.get_time() - start_time self._update_feedback(deepcopy(last), joint_names, now_from_start) # Verify goal constraint result = check_goal_state() if result is True: rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" % (self._action_name, self._name)) self._result.error_code = self._result.SUCCESSFUL self._server.set_succeeded(self._result) elif result is False: rospy.logerr("%s: Exceeded Max Goal Velocity Threshold for %s arm" % (self._action_name, self._name)) self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED self._server.set_aborted(self._result) else: rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" % (self._action_name, result, self._name)) self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED self._server.set_aborted(self._result) self._command_stop(goal.trajectory.joint_names, end_angles, start_time, dimensions_dict)
def spin(self): r = rospy.Rate(1.0/self.dt) while not rospy.is_shutdown(): if self.req_pos is None: r.sleep() continue self.lock.acquire() req_vel = copy.deepcopy(self.joint_req_vel) last_vel_command = copy.deepcopy(self.last_vel_command) act_position = copy.deepcopy(self.act_position) #req_pos = copy.deepcopy(self.req_pos) now = rospy.Time.now() if last_vel_command is None: self.lock.release() r.sleep() continue if (now - last_vel_command) > rospy.Duration(0.5): print "final positions" print act_position.positions self.last_vel_command = None self.lock.release() r.sleep() continue jt = JointTrajectory() jt.joint_names = self.joint_names jtp = JointTrajectoryPoint() jtp.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #tl = list(self.req_pos.positions) for idx in range(0,len(req_vel)): self.req_pos[idx] = self.req_pos[idx] + (req_vel[idx]*self.dt) jtp.positions[idx] = self.req_pos[idx] jtp.velocities[idx] = req_vel[idx] #jtp.accelerations[idx] = 150.0 jt.header.stamp = now jt.points.append(jtp) self.lock.release() self.pub.publish(jt) r.sleep()
cur_names, cur_pos = zip(*cur_state) # unzip end_pos = [] for p in cur_pos: end_pos.append(p + 0.2) traj_msg = JointTrajectory() traj_msg.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(2.0) traj_msg.joint_names = cur_names traj_msg.points = [] orig_point = JointTrajectoryPoint() orig_point.positions = cur_pos orig_point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] orig_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] orig_point.time_from_start = rospy.Duration.from_sec(0.0) traj_msg.points.append(orig_point) end_point = JointTrajectoryPoint() end_point.positions = end_pos end_point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] end_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] end_point.time_from_start = rospy.Duration.from_sec(2.0) traj_msg.points.append(end_point) print traj_msg traj_f = client.execute(traj_msg) print traj_f.result()
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint'] CONTROLLER_CMD_TOPIC = "/left_arm_controller/command" def get_n_randoms(n, min, max): randoms = [] max_value = max - min for i in range(n): randoms.append((random.random() * max_value) + min) # this scales the value between min and max return randoms if __name__ == '__main__': rospy.init_node('random_left_arm_goals') ctl_pub = rospy.Publisher(CONTROLLER_CMD_TOPIC, JointTrajectory) rospy.sleep(0.1) while not rospy.is_shutdown(): jt = JointTrajectory() jt.joint_names = JOINT_NAMES jtp = JointTrajectoryPoint() jtp.positions = get_n_randoms(len(JOINT_NAMES), -1.0, 4.0) jtp.velocities = [0.0] * len(JOINT_NAMES) jtp.accelerations = [0.0] * len(JOINT_NAMES) jtp.time_from_start = rospy.Duration(random.random() + 0.5) # 0.5s to 1.5s jt.points.append(jtp) ctl_pub.publish(jt) rospy.loginfo("Sent: " + str(jt)) rospy.sleep(jtp.time_from_start)
def getPlans(f, acHan): file_plans = [] plans = f.read().split("PLAN : ") plans = plans[1:] for p in plans: name_plan = p.split(' >\n') #print(name_plan[0]) el = name_plan[1].split('\n') if (len(el) < 5): file_plans.append(el[0]) else: x = RobotTrajectory() el = p.split('\n') #get frame id fram_id = el[7].split(": ")[1] x.joint_trajectory.header.frame_id = fram_id #get joint names joints = el[8].split(": ")[1][1:-1] joints = joints.split(", ") for j in range(0, len(joints)): joints[j] = joints[j].split('\'')[1] x.joint_trajectory.joint_names = joints #get points done = False counter = 10 while(not done): if ("multi" in el[counter]): done = True else: y = JointTrajectoryPoint() #get pos pos = el[counter + 1].split(": ")[1][1:-1] pos = pos.split(", ") for i in range(0, len(pos)): pos[i] = float(pos[i]) y.positions = pos #get vel vel = el[counter + 2].split(": ")[1][1:-1] vel = vel.split(", ") for i in range(0, len(vel)): vel[i] = float(vel[i]) y.velocities = vel #get acc acc = el[counter + 3].split(": ")[1][1:-1] acc = acc.split(", ") for i in range(0, len(acc)): acc[i] = float(acc[i]) y.accelerations = acc #get time st = el[counter + 6].split(": ")[1] et = el[counter + 7].split(": ")[1] y.time_from_start.secs = float(st) y.time_from_start.nsecs = float(et) x.joint_trajectory.points.append(y) counter = counter + 8 file_plans.append(x) return file_plans
def makePoint(position): mypoint = JointTrajectoryPoint() mypoint.positions = position mypoint.velocities = [ 0, 0, 0, 0, 0, 0, 0 ] mypoint.accelerations = [ 0, 0, 0, 0, 0, 0, 0 ] return mypoint