def callback(data): rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.position) pub = rospy.Publisher('arm_controller/command', JointTrajectory, queue_size=10) joints_str = JointTrajectory() joints_str.header = Header() joints_str.header.stamp = rospy.Time.now() joints_str.joint_names = ['base_joint', 'shoulder', 'elbow', 'wrist'] point=JointTrajectoryPoint() point.positions = [data.position[0], data.position[1], data.position[2], data.position[3]] point.time_from_start = rospy.Duration(2) joints_str.points.append(point) pub.publish(joints_str) pub = rospy.Publisher('gripper_controller/command', JointTrajectory, queue_size=10) joints_str = JointTrajectory() joints_str.header = Header() joints_str.header.stamp = rospy.Time.now() joints_str.joint_names = ['gripper'] point=JointTrajectoryPoint() point.positions = [data.position[4]] point.time_from_start = rospy.Duration(2) joints_str.points.append(point) pub.publish(joints_str) rospy.loginfo("position updated")
def publisher(): panda1_publisher = rospy.Publisher( 'panda1/position_joint_trajectory_controller/command', JointTrajectory, queue_size=1) panda2_publisher = rospy.Publisher( 'panda2/position_joint_trajectory_controller/command', JointTrajectory, queue_size=1) rospy.init_node('joint_pose_publisher', anonymous=True) rate = rospy.Rate(10) # 10hz panda1_trajectory = JointTrajectory() panda1_trajectory.joint_names = [ 'panda1_joint1', 'panda1_joint2', 'panda1_joint3', 'panda1_joint4', 'panda1_joint5', 'panda1_joint6', 'panda1_joint7' ] panda1_trajectory.points.append(JointTrajectoryPoint()) panda1_trajectory.points[0].time_from_start = rospy.Duration(1.0) panda1_trajectory.points[0].velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] panda1_trajectory.points[0].effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] panda1_trajectory.points[0].accelerations = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] panda2_trajectory = JointTrajectory() panda2_trajectory.joint_names = [ 'panda2_joint1', 'panda2_joint2', 'panda2_joint3', 'panda2_joint4', 'panda2_joint5', 'panda2_joint6', 'panda2_joint7' ] panda2_trajectory.points.append(JointTrajectoryPoint()) panda2_trajectory.points[0].time_from_start = rospy.Duration(1.0) panda2_trajectory.points[0].velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] panda2_trajectory.points[0].effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] panda2_trajectory.points[0].accelerations = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] while not rospy.is_shutdown(): panda1_trajectory.header = std_msgs.msg.Header() panda2_trajectory.header = std_msgs.msg.Header() panda1_trajectory.points[0].positions = [ 1.0, -1.0, 0.5, 0.5, 0.5, 0.0, 0.0 ] panda2_trajectory.points[0].positions = [ 1.0, -1.0, 0.5, 0.5, 0.5, 0.0, 0.0 ] panda1_publisher.publish(panda1_trajectory) panda2_publisher.publish(panda2_trajectory) rate.sleep()
def spin(self): rate = rospy.Rate(0.5) dmp_count = 1 n_dmps = len(glob.glob('./*.npy')) while not rospy.is_shutdown(): if self.header is not None: dmp = self.load_dmp(dmp_count) y_r, dy_r, _ = dmp.rollout() jt = JointTrajectory() jt.header = self.header jt.joint_names = dmp.joint_names jtp = JointTrajectoryPoint() for i in range(y_r.shape[0]): jtp.positions = y_r[i, :].tolist() jtp.velocities = dy_r[i, :].tolist() jtp.time_from_start = rospy.Duration(1) jt.points.append(jtp) self.traj_pub.publish(jt) print('Publishing dmp {}'.format(dmp_count)) dmp_count += 1 if dmp_count > n_dmps: break rate.sleep()
def main(): rospy.init_node('send_joints') pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR5 traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] rate = rospy.Rate(1) pts = JointTrajectoryPoint() traj.header.stamp = rospy.Time.now() while not rospy.is_shutdown(): for i in range(0,1): pts.positions = waypoints[i] pts.time_from_start = rospy.Duration(1.0) # Set the points to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj) rate.sleep()
def main(): rospy.init_node('send_joints') pub = rospy.Publisher('/trajectory_controller/command', JointTrajectory, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR5 traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] rate = rospy.Rate(10) while not rospy.is_shutdown(): traj.header.stamp = rospy.Time.now() pts = JointTrajectoryPoint() pts.positions = [0.180397344042591, -0.46159712847377854, 0.34107758901149676,-1.8290689486244203, -0.45481156632822817, 1.9918058620776087] pts.time_from_start = rospy.Duration(2.0) # Set the points to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj)
def spin(self): rate = rospy.Rate(0.5) dmp_count = 1 Ndmp = len(glob.glob('./*.npy')) while not rospy.is_shutdown(): if not (self.header == None): dmp = self.load_dmp(dmp_count) y_r, dy_r, ddy_r = dmp.rollout() #plt.subplot(1,2,1) #plt.plot(y_r) #plt.subplot(1,2,2) #plt.plot(dy_r) #plt.show() jt = JointTrajectory() jt.header = self.header jt.joint_names = dmp.joint_names jtp = JointTrajectoryPoint() for i in range(y_r.shape[0]): jtp.positions = y_r[i, :].tolist() jtp.velocities = dy_r[i, :].tolist() jtp.time_from_start = rospy.Duration(1) jt.points.append(jtp) self.traj_pub.publish(jt) print('Publishing dmp %d' % dmp_count) dmp_count += 1 if dmp_count > Ndmp: break rate.sleep()
def main(): rospy.init_node('send_joints') pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) sub = rospy.Subscriber('/arm_controller/state', JointTrajectoryControllerState, _observation_callback, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR5 traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] rate = rospy.Rate(10) while not rospy.is_shutdown(): traj.header.stamp = rospy.Time.now() pts = JointTrajectoryPoint() pts.positions = [0.0, 0.0, 1.0, 0.0, 0.0, 0.0] pts.time_from_start = rospy.Duration(0.1) # Set the points to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj)
def move(self, p, v_scale=0.1, duration_low_bound=1, start_duration=0.5): target_q = self.get_inverse_kin(p) if target_q is None: return False traj = JointTrajectory() traj.header = rospy.Header(frame_id="1", stamp=rospy.Time()) traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] dist = np.abs(np.subtract(target_q, self.current_state.positions)).max() duration = rospy.Duration( nsecs=int(max(dist / v_scale, duration_low_bound) * 1e9)) # print(dist,duration.secs,duration.nsecs) end_point = JointTrajectoryPoint( positions=target_q, velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start=duration) start_point = JointTrajectoryPoint( positions=(np.array(self.current_state.positions) + np.array(self.current_state.velocities) * (start_duration + 0.01)).tolist(), velocities=self.current_state.velocities, time_from_start=rospy.Duration(0, int(start_duration * 1e9))) traj.points = [start_point, end_point] self.pub.publish(traj) return True
def main(data): pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) # Create the topic message arm = JointTrajectory() arm.header = Header() # Joint names for UR5 arm.joint_names = [ 'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_gripper_right_joint', 'arm_gripper_left_joint' ] rate = rospy.Rate(50) # 50hz rospy.loginfo(arm) arm.header.stamp = rospy.Time.now() pts = JointTrajectoryPoint() pts.positions = [ data.position[3], data.position[0], data.position[4], data.position[1], data.position[2] ] pts.time_from_start = rospy.Duration(1.0) # Set the points to the trajectory arm.points = [] arm.points.append(pts) # Publish the message pub.publish(arm) rate.sleep()
def create_ee_traj(self, state): gripper_traj = JointTrajectory() gripper_current_states = self.request_joint_states('gripper') gripper_state = gripper_current_states[3] #we only care about the 3rd joint which is the active joint gripper_traj.header = Header() gripper_traj.header.frame_id = '/world' gripper_traj.joint_names = ['robotiq_85_left_knuckle_joint'] point_ = JointTrajectoryPoint() gripper_traj.points.append(point_) if self.gripper_state_initial is not None: #if this is the first operation gripper_traj.points[0].positions = [self.gripper_state_initial] self.gripper_state_initial = None elif self.gripper_state_current is not None: gripper_traj.points[0].positions = self.gripper_state_current else: rospy.logwarn('PlanningServiceClient::create_ee_traj() -- Damn dude, not even sure how you did that') point_ = JointTrajectoryPoint() if 'open' in state: # add open point to trajectory point_.positions = [0.0] self.gripper_state_current = [0.0] elif 'close' in state: #add closed point to trajectory point_.positions = [0.803] self.gripper_state_current = [0.803] else: rospy.logwarn('PlanningServiceClient::create_ee_traj() -- incorrect state sent') return False point_.time_from_start.secs = 2 # how long it will take to actuate gripper_traj.points.append(point_) rospy.loginfo('PlanningServiceClient::create_ee_traj() -- gripper traj = {}'.format(gripper_traj)) return gripper_traj
def joint_trajectory(self, joint_trajectory): ''' Returns just the part of the trajectory corresponding to the arm. **Args:** **joint_trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to convert **Returns:** A trajectory_msgs.msg.JointTrajectory corresponding to only this arm in joint_trajectory **Raises:** **ValueError:** If some arm joint is not found in joint_trajectory ''' arm_trajectory = JointTrajectory() arm_trajectory.header = copy.deepcopy(joint_trajectory.header) arm_trajectory.joint_names = copy.copy(self.joint_names) indexes = [-1]*len(self.joint_names) for (i, an) in enumerate(self.joint_names): for (j, n) in enumerate(joint_trajectory.joint_names): if an == n: indexes[i] = j break if indexes[i] < 0: raise ValueError('Joint '+n+' is missing from joint trajectory') for joint_point in joint_trajectory.points: arm_point = JointTrajectoryPoint() arm_point.time_from_start = joint_point.time_from_start for i in indexes: arm_point.positions.append(joint_point.positions[i]) arm_trajectory.points.append(arm_point) return arm_trajectory
def convert_trajectory(self, traj): """ Converts a trajectory into a joint trajectory Args: traj: Trajectory to convert Returns: joint trajectory """ new_traj = JointTrajectory() new_traj.header = traj.header # Take each joint in the trajectory and add it to the joint trajectory for joint_name in traj.joint_names: new_traj.joint_names.append(self.joint_map[joint_name].joint_name) # For each poiint in the trajectory for point in traj.points: new_point = JointTrajectoryPoint() for i, pos in enumerate(point.positions): new_point.positions.append(self.joint_map[new_traj.joint_names[i]].convert_angle(pos)) for i, vel in enumerate(point.velocities): new_point.velocities.append(self.joint_map[new_traj.joint_names[i]].convert_velocity(vel)) new_point.time_from_start = point.time_from_start new_traj.points.append(new_point) return new_traj
def joint_client(position): rospy.init_node('send_joints') pub = rospy.Publisher('/trajectory_controller/command', JointTrajectory, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR5 traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] rate = rospy.Rate(10) i = 0 while not rospy.is_shutdown(): traj.header.stamp = rospy.Time.now() pts = JointTrajectoryPoint() pts.time_from_start = rospy.Duration(1.0) pts.positions = position # Set the point to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj) if (i == 20000): break i = i + 1
def _transform_base_trajectory(self, base_traj, odom_to_frame_pose): odom_to_frame = geometry.pose_to_tuples(odom_to_frame_pose) num_points = len(base_traj.points) odom_base_traj = JointTrajectory() odom_base_traj.points = list( utils.iterate(JointTrajectoryPoint, num_points)) odom_base_traj.header = base_traj.header odom_base_traj.joint_names = self._base_joint_names # Transform each point into odom frame previous_theta = 0.0 for i in range(num_points): t = base_traj.points[i].transforms[0] frame_to_base = geometry.transform_to_tuples(t) # odom_to_base = odom_to_frame * frame_to_base (odom_to_base_trans, odom_to_base_rot) = geometry.multiply_tuples( odom_to_frame, frame_to_base) odom_base_traj.points[i].positions = [ odom_to_base_trans[0], odom_to_base_trans[1], 0 ] roll, pitch, yaw = T.euler_from_quaternion(odom_to_base_rot) dtheta = geometry.shortest_angular_distance(previous_theta, yaw) theta = previous_theta + dtheta odom_base_traj.points[i].positions[2] = theta previous_theta = theta return odom_base_traj
def main(): rospy.init_node('send_joints') rospy.loginfo("###########################################################") pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR5 traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] rate = rospy.Rate(1) pts = JointTrajectoryPoint() traj.header.stamp = rospy.Time.now() pts.positions = [0.0, -1.57, 1.57, -1.57, -1.57, 0.0] pts.time_from_start = rospy.Duration(1.0) # Set the points to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj) rospy.loginfo("-------------------------------------------------------------") rospy.spin()
def execute_cb(self, goal): if not self.equalJoints(goal.trajectory.joint_names): rospy.logerr("inaccurate joint names received") return if self._active_flag: nans = JointTrajectory() nans.header = goal.trajectory.header nans.joint_names = goal.trajectory.joint_names nans.points = JointTrajectoryPoint() nans.points.positions = [float("nan")] * len(nans.joint_names) print "execute" + rospy.get_name() self._finishable = False command = JointTrajectory() command.header = goal.trajectory.header command.joint_names = goal.trajectory.joint_names command.points = [ goal.trajectory.points[-1], ] self._active_goal = goal self._active_command = command self._active_end_time = rospy.get_rostime() + goal.trajectory.points[ -1].time_from_start + goal.goal_time_tolerance + rospy.Duration( 2, 0) #2.0 sec is nantonaku self._pub_command.publish(command) self._active_flag = True self._execute_state = True r = rospy.Rate(20) while not rospy.is_shutdown(): if self._finishable: break r.sleep() self._active_flag = False if not self._execute_state: self._result.error_string = "execution timeout" self._as.set_aborted(self._result) return rospy.loginfo('%s: Succeeded' % self._action_name) self._result.error_string = "execution succeeded" self._result.error_code = 0 self._as.set_succeeded(self._result)
def jointTrajectory(self, ID, jointNames, points): # Command a joint trajectory of arm msg = JointTrajectory() msg.header = Header() msg.header.frame_id = ID msg.joint_names = jointNames msg.points = [] msg.points.append(points) # Publish self.jointTrajectoryPublisher.publish(msg)
def main(): os.makedirs(os.path.join(assets_dir(), 'data', '{}_ur10_train_data'.format(args.tag))) rospy.init_node('send_joints') # create a publish node named send_joints pub = rospy.Publisher('/trajectory_controller/command', JointTrajectory, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] rate = rospy.Rate(10) f_joint = open(os.path.join(assets_dir(), 'data/{}_ur10_train_data/joint_data.txt'.format(args.tag)),"a+") f_end_effector = open(os.path.join(assets_dir(), 'data/{}_ur10_train_data/end_effector_data.txt'.format(args.tag)),"a+") iteration = 0 while not rospy.is_shutdown(): traj.header.stamp = rospy.Time.now() pts = JointTrajectoryPoint() random_pos_shoulder_lift_joint = np.random.uniform(-3.14,0) # set shoulder_lift_joint angle random_pos = np.random.uniform(-3.14,3.14,size=(5)) # set other joints angle random_pos = np.insert(random_pos,1, random_pos_shoulder_lift_joint) print("random_pos",random_pos) pts.positions = random_pos pts.time_from_start = rospy.Duration(1.0) # Set the points to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj) time.sleep(5) # Delays for 5 seconds. x_position, y_position, z_position = pos_main() end_effector_position = np.array([x_position, y_position, z_position]) # pickle store data process...... # pickle.dump(random_pos, open(os.path.join(assets_dir(), # 'data/{}_ur5_train_data/joint_angle_data.pkl'.format(args.tag)), 'wb'), # protocol=pickle.HIGHEST_PROTOCOL) # pickle.dump((x_position, y_position, z_position), open(os.path.join(assets_dir(), # 'data/{}_ur5_train_data/end_effector_position_data.pkl'.format(args.tag)), 'wb'), # protocol=pickle.HIGHEST_PROTOCOL) print >> f_joint, random_pos print >> f_end_effector, end_effector_position iteration += 1 print("iteration",iteration) if iteration == 1000: break
def get_joint_trajectory_msg(self, path_config): trajectory = JointTrajectory() trajectory.header = self.group.get_current_pose().header trajectory.joint_names = self._get_joint_names() time_from_start = 0 for config in path_config: trajectory.points.append( self.get_joint_trajectory_point_msg(config, time_from_start)) time_from_start += 0.5 return trajectory
def main(): rospy.init_node('send_joints') pub = rospy.Publisher('/trajectory_controller/command', JointTrajectory, queue_size=10) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR5 traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] reader.next() rate = rospy.Rate(50) while not rospy.is_shutdown(): traj.header.stamp = rospy.Time.now() pts = JointTrajectoryPoint() # for row in reader: # shoulder_pan_joint = (row[0]) # shoulder_lift_joint = (row[1]) # elbow_joint = (row[2]) # wrist_1_joint = (row[3]) # wrist_2_joint = (row[4]) # wrist_3_joint = (row[5]) # pts.positions = [shoulder_pan_joint, shoulder_lift_joint, \ # elbow_joint, wrist_1_joint, wrist_2_joint, \ # wrist_3_joint] # pts.time_from_start = rospy.Duration(0.02) # # Set the points to the trajectory # traj.points = [] # traj.points.append(pts) # # Publish the message # pub.publish(traj) pts.positions = [ 1.2778283886, -0.6807085686, -0.2895342907, -0.5970439271, -0.2896611142, 8.45356745875492E-05 ] pts.time_from_start = rospy.Duration(0.02) # Set the points to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj)
def push_one_arm(self, new_plan, left_arm_joints): # Build a new joint trajectory with left_arm_joint values at those times temp_traj = JointTrajectory() new_traj = new_plan.joint_trajectory temp_traj.header = new_traj.header temp_traj.joint_names = self.traj.joint_names zero_vec = [0.0]*7 for i in range(len(new_traj.points)): temp_traj.points[i].positions = left_arm_joints.values() + new_traj.points[i].positions temp_traj.points[i].velocities = zero_vec + new_traj.points[i].velocities temp_traj.points[i].accelerations = zero_vec + new_traj.points[i].accelerations temp_traj.points[i].time_from_start = new_traj.points[i].time_from_start self.traj = temp_traj
def build_jt(header, joint_names, y_r, dy_r): jt = JointTrajectory() jt.header = header jt.joint_names = joint_names jtp = JointTrajectoryPoint() for i in range(y_r.shape[0]): jtp.positions = y_r[i, :].tolist() jtp.velocities = dy_r[i, :].tolist() jtp.time_from_start = rospy.Duration(1.0) jt.points.append(jtp) return jt
def move_all_joints(self, point): joints_str = JointTrajectory() joints_str.header = Header() joints_str.header.stamp = rospy.Time.now() joints_str.joint_names = [ 'neck_1', 'neck_2', 'neck_3', 'neck_4', 'neck_5', 'shoulder_l1', 'shoulder_l2', 'biceps_l', 'elbow_l', 'forearm_l', 'shoulder_r1', 'shoulder_r2', 'biceps_r', 'elbow_r', 'forearm_r' ] joints_str.points.append(point) self.goal_dynamixel_position_publisher.publish(joints_str)
def min_jerk(self, setpoint, frequency, move_time): current = np.array(self.joint_states_msg.position) timefreq = int(move_time * frequency) trajectory = [] trajectory_derivative = [] rospy.logwarn("Min_Jerk trajectory generation...") for time in range(1, timefreq): trajectory.append(current + (setpoint - current) * (10.0 * (time / timefreq)**3 - 15.0 * (time / timefreq)**4 + 6.0 * (time / timefreq)**5)) trajectory_derivative.append( frequency * (1.0 / timefreq) * (setpoint - current) * (30.0 * (time / timefreq)**2.0 - 60.0 * (time / timefreq)**3.0 + 30.0 * (time / timefreq)**4.0)) joints_str = JointTrajectory() joints_str.header = Header() joints_str.header.stamp = rospy.Time.now() joints_str.joint_names = [ 'neck_1', 'neck_2', 'neck_3', 'neck_4', 'neck_5', 'shoulder_l1', 'shoulder_l2', 'biceps_l', 'elbow_l', 'forearm_l', 'shoulder_r1', 'shoulder_r2', 'biceps_r', 'elbow_r', 'forearm_r' ] rospy.logwarn("Waypoints generation...") #Adding the point to the points list for i in range(1, timefreq - 1): point = JointTrajectoryPoint() point.positions = np.around(trajectory[i], 2) rospy.loginfo(point.positions) point.velocities = np.around(trajectory_derivative[i], 2) rospy.loginfo(point.velocities) point.time_from_start = rospy.Duration( move_time) ##dipende da quanti punti prendiamo? testare joints_str.points.append(point) self.goal_dynamixel_position_publisher.publish(joints_str) rospy.logwarn("Trajectory published!") rospy.loginfo(joints_str)
def go_position(goal): pub = rospy.Publisher('/trajectory_controller/command',JointTrajectory,queue_size=10) goal_x=goal.fpos_x goal_y=goal.fpos_y print("goal_x:",goal_x) print("goal_y:",goal_y) #goal_z=goal.fpos_z now_position=np.array([0,0]) # Create the topic message traj = JointTrajectory() traj.header = Header() # Joint names for UR5 traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint','elbow_joint','wrist_3_joint'] rate = rospy.Rate(10) #th1, th2, nx, ny, nz, ox, oy, oz, ax, ay, az, Px, Py, Pz, a1, a2=symbols('th1,th2,nx,ny,nz,ox,oy,oz,ax,ay,az,Px,Py,Pz,a1,a2') while not rospy.is_shutdown(): #input position, output angle algori #RotZ1 = np.array([[cos(th1), sin(th1), 0, 0],[sin(th1), cos(th1), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) #RotZ2 = np.array([[cos(th2), -sin(th2), 0, 0], [sin(th2), cos(th2), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) #TransA1 = np.array([[1, 0, 0, a1], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) #TransA2 = np.array([[1, 0, 0, a2], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) #T_0_2 = RotZ1*TransA1*RotZ2*TransA2 #TT = np.array([[nx, ox, ax, Px], [ny, oy, ay, Py], [nz, oz, az, Pz], [0, 0, 0, 1]]) #LS1 = simplify(np.linalg.inv(RotZ1)*TT) #RS1 = simplify(np.linalg.inv(RotZ1)*T_0_2) #assume link lenght a1=5 #link1 length a2=3 #link2 length Px=goal_x #goal_x Py=goal_y #goal_y th2 = np.array([2*atan((float((a1+a2)**2-(Px**2+Py**2))/float(Px**2+Py**2-(a1-a2)**2))**0.5), -2*atan((float((a1+a2)**2-(Px**2+Py**2))/float(Px**2+Py**2-(a1-a2)**2))**0.5)]) th1 = np.array([atan2(float(Py),float(Px)) - atan(float(a2*sin(th2[0]))/float(a1+a2*cos(th2[0]))),atan2(float(Py),float(Px)) - atan(float(a2*sin(th2[1]))/float(a1+a2*cos(th2[1])))]) traj.header.stamp = rospy.Time.now() pts = JointTrajectoryPoint() #choice th1[1],th2[1] // th1[2],th2[2] pts.positions = [th1[1],th2[1] ,0.0 , 0.0] pts.time_from_start = rospy.Duration(1.0) # Set the points to the trajectory traj.points = [] traj.points.append(pts) # Publish the message pub.publish(traj) result=godestinationResult() result.fpos_x=now_position[0] result.fpos_y=now_position[1] result.task=True server.set_succeeded(result,"destination completed successfully")
def callback(self, msg): pub_msg = JointTrajectory() pub_msg.header = msg.header pub_msg.joint_names = ['rhand_thumb_roll', 'rhand_thumb_pitch', 'rhand_middle_pitch'] rhand_thumb_roll_id = msg.goal.trajectory.joint_names.index('R_THUMB_JOINT0') rhand_thumb_pitch_id = msg.goal.trajectory.joint_names.index('R_THUMB_JOINT1') rhand_middle_pitch_id = msg.goal.trajectory.joint_names.index('R_MIDDLE_JOINT0') pub_msg.points = [] for p in msg.goal.trajectory.points: point = JointTrajectoryPoint() point.positions = [p.positions[head_pitch_id], p.positions[head_yaw_id]]#TOFIX point.velocities = [p.velocities[head_pitch_id], p.velocities[head_yaw_id]] point.time_from_start = p.time_from_start pub_msg.points.append(point) self.joint_command_pub.publish(pub_msg)
def publishArmControllerCommand(self, waypoints): """ Publishes a list of waypoints of given positions, velocities and durations to the trajectory controller Structure waypoints = [ { 'positions': [] 'velocities': [] 'duration': Float } ] """ # Create the topic message traj = JointTrajectory() traj.header = Header() traj.header.stamp = rospy.Time.now() # Joint names for UR5 traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] t = 0.0 traj.points = [] for waypoint in waypoints: t += waypoint['duration'] pts = JointTrajectoryPoint() pts.positions = waypoint['positions'] pts.velocities = waypoint['velocities'] pts.time_from_start = rospy.Duration(t) traj.points.append(pts) self._publishers['arm_controller_command'].publish(traj) # Sleep for duration motionIsFinished = False counter = 0 while not motionIsFinished and not rospy.is_shutdown(): if counter >= (t + 0.1) * SLEEP_RATE: motionIsFinished = True counter += 1 self._rate.sleep() return
def _add_motion_command_to_queue(self, command): tmp = JointTrajectory() tmp.header = command.header tmp.joint_names = ['pan_joint', 'pan_joint'] tmp.points = [JointTrajectoryPoint()] tmp.points[0].positions = [ command.pan_cmd.pos_rad, command.tilt_cmd.pos_rad ] tmp.points[0].velocities = [ command.pan_cmd.vel_rps, command.tilt_cmd.vel_rps ] tmp.points[0].accelerations = [ command.pan_cmd.acc_rps2, command.tilt_cmd.acc_rps2 ] self._cmd_buffer.put(tmp.points[0])
def issue_commands(publishers): for pub in publishers: point = JointTrajectoryPoint() point.positions = [ random() * 2 * np.pi, random() + 0.1, random() * 0.05 ] point.time_from_start = rospy.Duration( 1) # max(dur) / self._speed_scale cmd = JointTrajectory() cmd.header = Header() cmd.joint_names = ['joint1', 'joint2', 'joint5'] cmd.points = [point] pub.publish(cmd)
def push_one_arm(self, new_plan, left_arm_joints): # Build a new joint trajectory with left_arm_joint values at those times temp_traj = JointTrajectory() new_traj = new_plan.joint_trajectory temp_traj.header = new_traj.header temp_traj.joint_names = self.traj.joint_names zero_vec = [0.0] * 7 for i in range(len(new_traj.points)): temp_traj.points[i].positions = left_arm_joints.values( ) + new_traj.points[i].positions temp_traj.points[ i].velocities = zero_vec + new_traj.points[i].velocities temp_traj.points[ i].accelerations = zero_vec + new_traj.points[i].accelerations temp_traj.points[i].time_from_start = new_traj.points[ i].time_from_start self.traj = temp_traj
def talker(): pub_traj = rospy.Publisher('joint_path_command', JointTrajectory) rospy.init_node('path_publisher') traj = JointTrajectory() traj.header = Header(frame_id='base_link', stamp=rospy.Time.now() + rospy.Duration(1.0)) traj.joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] traj.points = [JointTrajectoryPoint(positions=[0, 0, 0, 0, 0, 0], velocities=[0, 0, 0, 0, 0, 0], time_from_start=rospy.Duration(1)), JointTrajectoryPoint(positions=[1, 0, 0, 0, 0, 0], velocities=[0, 0, 0, 0, 0, 0])] rospy.loginfo(traj) pub_traj.publish(traj) rospy.sleep(1.0)
def talker(): pub = rospy.Publisher('arm_controller/command', JointTrajectory, queue_size=10) rospy.init_node('trajectory') rate = rospy.Rate(10) # 10hz joints_str = JointTrajectory() joints_str.header = Header() joints_str.header.stamp = rospy.Time.now() joints_str.joint_names = ['base_joint', 'shoulder', 'elbow', 'wrist'] point=JointTrajectoryPoint() point.positions = [0.5, 0.5, 0.5, 0.5] point.time_from_start = rospy.Duration(2) joints_str.points.append(point) while not rospy.is_shutdown(): joints_str.header.stamp = rospy.Time.now() pub.publish(joints_str) rospy.loginfo("position updated") rate.sleep()
def start(self): self.start = rospy.Time.now() rate = rospy.Rate(10) s = 0 while not (rospy.is_shutdown() or s >= len(self.x_discretized)): # Build JointTrajectory message header = Header() header.seq = s header.stamp = rospy.get_rostime() header.frame_id = 'inertial frame' joint_trajectory_msg = JointTrajectory() joint_trajectory_msg.header = header joint_trajectory_msg.joint_names = ['drone'] # Build JointTrajectoryPoint for i in range(min(self.WINDOW_FRAME, len(self.x_discretized) - s)): joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [ self.x_discretized[s + i], self.y_discretized[s + i], self.z_discretized[s + i], self.ya_discretized[s + i] ] joint_trajectory_point.velocities = [ self.vx_discretized[s + i], self.vy_discretized[s + i], self.vz_discretized[s + i] ] if i != (self.WINDOW_FRAME - 1) else [.0, .0, .0] joint_trajectory_point.accelerations = [ self.ax_discretized[s + i], self.ay_discretized[s + i], self.az_discretized[s + i] ] if i != (self.WINDOW_FRAME - 1) else [.0, .0, .0] joint_trajectory_point.effort = [None] joint_trajectory_point.time_from_start = self.ti_discretized[s + i] joint_trajectory_msg.points.append(joint_trajectory_point) s = s + 1 # trajectory = "new trajectory %s" % rospy.get_time() rospy.loginfo('##########################################') rospy.loginfo(joint_trajectory_msg) self.pub.publish(joint_trajectory_msg) rate.sleep()
def set_joints_manually(self, joint_angles, joint_names, time): joint_angles_np = np.array(joint_angles) zeros_list = (joint_angles_np * 0).tolist() joint_angles_list = joint_angles_np.tolist() h = Header() h.stamp = rospy.Time.now( ) # Note you need to call rospy.init_node() before this will work. This needs to be any number in the future pos = joint_angles vel = zeros_list acc = zeros_list eff = zeros_list dur = rospy.Duration(time) point = JointTrajectoryPoint(pos, vel, acc, eff, dur) trajectory = JointTrajectory() trajectory.joint_names = joint_names trajectory.points = [point] trajectory.header = h self.direct_joint_command_pub.publish(trajectory)
def substitute_trajectory(self, trajectory): jt = JointTrajectory() jt.joint_names = self.real_joint_names jt.header = trajectory.header for jtp_in in trajectory.points: jtp = JointTrajectoryPoint() if jtp_in.positions: joint_position = jtp_in.positions[0] # Each finger will go to half of the size jtp.positions = [joint_position / 2.0, joint_position / 2.0] jtp.time_from_start = jtp_in.time_from_start else: rospy.logwarn("Trajectory has no positions...") if jtp_in.velocities: jtp.velocities.append(jtp_in.velocities[0]) if jtp_in.accelerations: jtp.accelerations.append(jtp_in.accelerations[0]) if jtp_in.effort: jtp.effort.append(jtp_in.effort[0]) jt.points.append(jtp) return jt
def functional(axis, angle): pub_rosey = rospy.Publisher( '/cyton_joint_trajectory_action_controller/command', JointTrajectory, queue_size=10) rospy.init_node('traj_maker', anonymous=True) time.sleep(1) axis = int(axis) angle = float(angle) rate = rospy.Rate(0.01) while not rospy.is_shutdown(): # first way to define a point traj_waypoint_1_rosey = JointTrajectoryPoint() traj_waypoint_1_rosey.positions = [0,0,0,0,0,0,0] traj_waypoint_1_rosey.time_from_start = Duration(2) # second way to define a point traj_waypoint_2_rosey = traj_waypoint_1_rosey traj_waypoint_2_rosey.time_from_start = Duration(4) traj_waypoint_2_rosey.positions[axis-1] = angle time.sleep(1) traj_waypoint_3_rosey = traj_waypoint_2_rosey traj_waypoint_3_rosey.time_from_start = Duration(7) #traj_waypoint_2_rosey = JointTrajectoryPoint(positions=[.31, -.051, .33, -.55, .28, .60,0], #time_from_start = Duration(4)) #traj_waypoint_3_rosey = JointTrajectoryPoint(positions=[.14726, -.014151, .166507, -.33571, .395997, .38657,0], #time_from_start = Duration(6)) #traj_waypoint_4_rosey = JointTrajectoryPoint(positions=[-.09309, .003150, .003559, .16149, .524427, -.1867,0], #time_from_start = Duration(8)) #traj_waypoint_5_rosey = JointTrajectoryPoint(positions=[-.27752, .077886, -.1828, .38563, .682589, -.44665,0], #time_from_start = Duration(10)) #traj_waypoint_6_rosey = JointTrajectoryPoint(positions=[0,0,0,0,0,0,0],time_from_start = Duration(12)) #debug in terminal print traj_waypoint_2_rosey.positions # making message message_rosey = JointTrajectory() # required headers header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now()) message_rosey.header = header_rosey # adding in joints joint_names = ['shoulder_roll_joint', \ 'shoulder_pitch_joint', 'shoulder_yaw_joint', 'elbow_pitch_joint', \ 'elbow_yaw_joint', 'wrist_pitch_joint', 'wrist_roll_joint'] message_rosey.joint_names = joint_names # appending up to 100 points # ex. for i in enumerate(len(waypoints)): append(waypoints[i]) message_rosey.points.append(traj_waypoint_1_rosey) message_rosey.points.append(traj_waypoint_2_rosey) message_rosey.points.append(traj_waypoint_3_rosey) #message_rosey.points.append(traj_waypoint_4_rosey) #message_rosey.points.append(traj_waypoint_5_rosey) #message_rosey.points.append(traj_waypoint_6_rosey) # publishing to ROS node pub_rosey.publish(message_rosey) rate.sleep() if rospy.is_shutdown(): break