def callback(self, msg): pub_msg = FollowJointTrajectoryActionGoal() pub_msg.header = msg.header pub_msg.goal_id = msg.goal_id pub_msg.goal.trajectory.header = msg.goal.trajectory.header pub_msg.goal.trajectory.joint_names = [ 'r_thumb_roll', 'r_thumb_pitch', 'r_middle_pitch' ] thumb_r_id = msg.goal.trajectory.joint_names.index('THUMB_R') thumb_p_id = msg.goal.trajectory.joint_names.index('THUMB_P') middle_p_id = msg.goal.trajectory.joint_names.index('MIDDLE_P') pub_msg.goal.trajectory.points = [] for p in msg.goal.trajectory.points: point = JointTrajectoryPoint() point.positions = [ p.positions[thumb_r_id], p.positions[thumb_p_id], p.positions[middle_p_id] ] point.velocities = [ p.velocities[thumb_r_id], p.velocities[thumb_p_id], p.velocities[middle_p_id] ] point.time_from_start = p.time_from_start pub_msg.goal.trajectory.points.append(point) self.joint_command_pub.publish(pub_msg)
def goDownInShelf(self, x, y, z, alpha, beta, gamma): traj = JointTrajectory() traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] now = rospy.get_rostime() rospy.loginfo("Current time %i %i", now.secs, now.nsecs) traj.header.stamp = now ag = FollowJointTrajectoryActionGoal() ag.goal.trajectory = traj touching_item = JointTrajectoryPoint() touching_item.positions = [ math.radians(56.28), math.radians(-82.90), math.radians(99.17), math.radians(-107.89), math.radians(271.42), math.radians(-38.20) ] touching_item.velocities = self.default_velocity touching_item.time_from_start = rospy.Duration(10.0) traj.points.append(touching_item) ag = FollowJointTrajectoryActionGoal() ag.goal.trajectory = traj self.joint_angle_pub.publish(ag) rospy.sleep(10)
def publish_msg(self): # Set the message to publish as command. traj_vector = FollowJointTrajectoryActionGoal() # Current ROS time stamp h = Header() h.stamp = rospy.Time.now() traj_vector.header = h traj_vector.goal.trajectory.joint_names.append('arm_1_joint') traj_vector.goal.trajectory.joint_names.append('arm_2_joint') traj_vector.goal.trajectory.joint_names.append('arm_3_joint') traj_vector.goal.trajectory.joint_names.append('arm_4_joint') traj_vector.goal.trajectory.joint_names.append('arm_5_joint') traj_vector.goal.trajectory.joint_names.append('arm_6_joint') traj_vector.goal_id.stamp = h.stamp traj_vector.goal_id.id = 'rosPathPlanner' + str(h.stamp.secs) h2 = Header() h2.stamp.secs = h.stamp.secs + 1 h2.stamp.nsecs = h.stamp.nsecs + 3e8 traj_vector.goal.trajectory.header.stamp = h2.stamp points2pub = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] iter = 0.0 for column in points2pub: point = JointTrajectoryPoint() iter += 1 for q in column: point.positions.append(q) point.time_from_start.nsecs = 0 point.time_from_start.secs = iter * 10 traj_vector.goal.trajectory.points.append(point) print traj_vector print 'All systems go!' self.trajPub.publish(traj_vector)
def goUpInShelf(self, x, y, z, alpha, beta, gamma): traj = JointTrajectory() traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] now = rospy.get_rostime() rospy.loginfo("Current time %i %i", now.secs, now.nsecs) traj.header.stamp = now ag = FollowJointTrajectoryActionGoal() ag.goal.trajectory = traj inside = JointTrajectoryPoint() inside.positions = [ math.radians(56.51), math.radians(-85.19), math.radians(95.29), math.radians(-100.09), math.radians(270.91), math.radians(-38.14) ] inside.velocities = self.default_velocity inside.time_from_start = rospy.Duration(10.0) traj.points.append(inside) ag = FollowJointTrajectoryActionGoal() ag.goal.trajectory = traj self.joint_angle_pub.publish(ag) rospy.sleep(10)
def data_loop(self, data): target_angles = eval(data.decode()) trajectory_msg = self.generate_trajectory_msg(target_angles) goal_msg = FollowJointTrajectoryGoal() goal_msg.trajectory = trajectory_msg action_goal_msg = FollowJointTrajectoryActionGoal() action_goal_msg.header = trajectory_msg.header action_goal_msg.goal = goal_msg self.pub.publish(action_goal_msg)
def gotoBin(self, binName): traj = JointTrajectory() traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] bin = JointTrajectoryPoint() bin.velocities = self.default_velocity bin.time_from_start = rospy.Duration(5.0) if (binName == "A"): rospy.loginfo("Going to Shelf A!") bin.positions = self.shelfA_position elif (binName == "B"): rospy.loginfo("Going to Shelf B") bin.positions = self.shelfB_position elif (binName == "C"): rospy.loginfo("Going to Shelf C") bin.positions = self.shelfC_position elif (binName == "D"): rospy.loginfo("Going to Shelf D") bin.positions = self.shelfD_position elif (binName == "E"): rospy.loginfo("Going to Shelf E") bin.positions = self.shelfE_position elif (binName == "F"): rospy.loginfo("Going to Shelf F") bin.positions = self.shelfF_position elif (binName == "G"): rospy.loginfo("Going to Shelf G") bin.positions = self.shelfG_position elif (binName == "H"): rospy.loginfo("Going to Shelf H") bin.positions = self.shelfH_position elif (binName == "I"): rospy.loginfo("Going to Shelf I") bin.positions = self.shelfI_position elif (binName == "J"): rospy.loginfo("Going to Shelf J") bin.positions = self.shelfJ_position elif (binName == "K"): rospy.loginfo("Going to Shelf K") bin.positions = self.shelfK_position elif (binName == "L"): rospy.loginfo("Going to Shelf L") bin.positions = self.shelfL_position else: bin.positions = self.home_position traj.points.append(bin) now = rospy.get_rostime() rospy.loginfo("Current time %i %i", now.secs, now.nsecs) traj.header.stamp = now ag = FollowJointTrajectoryActionGoal() ag.goal.trajectory = traj self.joint_angle_pub.publish(ag) rospy.sleep(5)
def talker(): rospy.loginfo('starting the talker node') global counter rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz pub = rospy.Publisher('/arm_controller/follow_joint_trajectory/goal', FollowJointTrajectoryActionGoal, queue_size=10) rospy.sleep(2) command = FollowJointTrajectoryActionGoal() command.header.stamp = rospy.Time.now() command.goal.trajectory.joint_names = ['elbow'] point = JointTrajectoryPoint() point.positions = [math.pi / 2] point.velocities = [1.5] #0.5 #point.positions = [math.sin(counter/4)*math.pi] # in which range is sinus defined? point.time_from_start = rospy.Duration(10) command.goal.trajectory.points.append(point) pub.publish(command) rospy.loginfo('=====send command %r', command.goal.trajectory.points[0]) rospy.sleep(2) counter += 1 rate.sleep()
def execute_trajectory(self, traj): if self._execution_running: rospy.logwarn( 'Request to execute a trajectory while an old one is already running' ) self._action_client.cancel_goal() self._execution_running = False traj.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.1) self._current_arm_traj = ROSUtils.extract_part_trajectory( self._arm_joint_names, traj) self._current_hand_traj = ROSUtils.extract_part_trajectory( self._hand_joint_names, traj) self._current_traj = traj arm_traj_goal = FollowJointTrajectoryActionGoal() arm_traj_goal.goal.trajectory = self._current_arm_traj arm_traj_goal.goal.goal_tolerance = self._goal_tolerance arm_traj_goal.goal.path_tolerance = self._path_tolerance # TODO for some reason we do not receive feedback, so we add a fake feedback caller # TODO here self._action_client.send_goal( arm_traj_goal.goal, active_cb=self._receive_arm_traj_start_signal, feedback_cb=self._receive_arm_traj_feedback, done_cb=self._receive_arm_traj_finish_signal) self._fake_feedback_timer = rospy.Timer(rospy.Duration.from_sec(0.05), self._simulate_feedback, oneshot=False) # TODO return whether we grasped an object return True
def gotoxy_handle(req): move = FollowJointTrajectoryActionGoal() x= req.x y = req.y#float(sys.argv[1]) offset = 0.048 X= y - offset Y= -x l1= 0.08 l2= 0.047 theta2 = math.acos((X*X + Y*Y - l1*l1 - l2*l2)/(2*l1*l2)) alpha = math.atan2(Y,X) beta = math.acos((X*X + Y*Y + l1*l1 - l2*l2)/(2*l1*math.sqrt(X*X + Y*Y))) theta1 = alpha - beta point = JointTrajectoryPoint() point.positions = [theta1,-theta2] point.time_from_start = rospy.Duration(3) move.goal.trajectory.joint_names = ['shoulder_1_joint', 'shoulder_2_joint'] move.goal.trajectory.points.append(point) client.send_goal(move.goal) client.wait_for_result(rospy.Duration.from_sec(5.0)) return True
def cmdJointState(self, joint_pos, time=5.0): ''' To command a joint position to the hand.. ''' self.switch_operation_mode('position') rospy.sleep(0.02) if self.joint_names is not None: point = JointTrajectoryPoint() point.positions = joint_pos point.time_from_start.secs = time traj = JointTrajectory() traj.header.stamp = rospy.Time.now() traj.joint_names = self.joint_names traj.points.append(point) action_goal = FollowJointTrajectoryActionGoal() action_goal.header.stamp = rospy.Time.now() action_goal.goal.trajectory = traj action_goal.goal.goal_time_tolerance.secs = time self.cmd_joint_state_pub.publish(action_goal) rospy.sleep(time) self.switch_operation_mode('velocity') rospy.sleep(0.02)
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', 'joint_7', 'joint_8', 'joint_9', 'joint_a' ] for i in range(len(traj)): point = JointTrajectoryPoint() p = [0] * self.joint_num v = [0] * self.joint_num last_v = [0] * self.joint_num a = [0] * self.joint_num if i != len(traj) - 1 and i != 0: for j in range(self.joint_num): 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 main(): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node("ik_moveit") pub = rospy.Publisher('/arm_controller/follow_joint_trajectory/goal', FollowJointTrajectoryActionGoal, queue_size=1) compute_ik_srv = rospy.ServiceProxy("/compute_ik", GetPositionIK) rospy.wait_for_service("/compute_ik") arm_move_group = moveit_commander.MoveGroupCommander("manipulator") robot_commander = moveit_commander.RobotCommander() # compute reference position of the gripper ( link) ref_pose = pq_to_pose([0.256, 0.294, 0.365], [0.707, 0.707, 0.0, 0.0]) rate = rospy.Rate(10.0) iter_no = 0 max_iter = 10 while not rospy.is_shutdown(): # set reference position of the gripper ( link) pose_ik = PoseStamped() pose_ik.header.frame_id = "base_link" pose_ik.header.stamp = rospy.Time.now() pose_ik.pose = ref_pose req = GetPositionIKRequest() req.ik_request.group_name = "manipulator" req.ik_request.robot_state = robot_commander.get_current_state() req.ik_request.avoid_collisions = True req.ik_request.ik_link_name = arm_move_group.get_end_effector_link() req.ik_request.pose_stamped = pose_ik # print(arm_move_group.get_end_effector_link()) ik_response = compute_ik_srv(req) if ik_response.error_code.val == 1: print('Goal state:') print(ik_response.solution.joint_state.position) goal_pose = FollowJointTrajectoryActionGoal() # print (arm_move_group.get_joints()) goal_pose.goal.trajectory.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] goal_pose.goal.trajectory.points.append(JointTrajectoryPoint()) goal_pose.goal.trajectory.points[ 0].positions = ik_response.solution.joint_state.position goal_pose.goal.trajectory.points[0].velocities = [0, 0, 0, 0, 0, 0] goal_pose.goal.trajectory.points[ 0].time_from_start = rospy.Duration.from_sec(1.0) pub.publish(goal_pose) else: print('Could not find solution to inverse kinematic') rate.sleep() iter_no = iter_no + 1 if iter_no > max_iter: break
def arm_hand_action_control(self, q_goal): # JointTrajectory joint_goal = self.set_arm_hand_joint_goal(q_goal) # FollowJointTrajectoryActionGoal action_goal = FollowJointTrajectoryActionGoal() action_goal.goal.trajectory = joint_goal # send goal self.client.send_goal(action_goal.goal)
def move(self, pos): msg = FollowJointTrajectoryActionGoal() msg.goal.trajectory.joint_names = self.joint_name_lst point = JointTrajectoryPoint() point.positions = pos point.time_from_start = rospy.Duration(1.0 / 60.0) msg.goal.trajectory.points.append(point) self.jta.send_goal_and_wait(msg.goal)
def command(): if len(sys.argv) != 7: print "Usage: ", sys.argv[ 0], "<angle_joint_1> <angle_joint_2> <angle_joint_3> <angle_joint_4> <angle_joint_5> <angle_joint_6>" return -1 else: global flag global actual_config flag = False rospy.init_node('configuration_commander', anonymous=True) # rospy.loginfo("SERVER") rospy.Subscriber("/joint_states", JointState, state_callback) pub = rospy.Publisher('/joint_trajectory_action/goal', FollowJointTrajectoryActionGoal, queue_size=10) rospy.sleep(1) if flag: # Starting configuration initialization based on the joint state stored by state_callback start_config = JointTrajectoryPoint() start_config.positions = actual_config start_config.time_from_start.secs = 0 start_config.time_from_start.nsecs = 0 # Goal configuration initialization goal_config = JointTrajectoryPoint() goal_config_array = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] for i in range(0, 6): goal_config_array[i] = float(sys.argv[i + 1]) print goal_config_array[i] goal_config.positions = goal_config_array goal_config.time_from_start.secs = 1 goal_config.time_from_start.nsecs = 63310604 # Message inizialization traj_msg = FollowJointTrajectoryActionGoal() traj_msg.goal.trajectory.header.seq = 0 traj_msg.goal.trajectory.header.stamp.nsecs = 0 traj_msg.goal.trajectory.header.stamp.secs = 0 traj_msg.goal.trajectory.header.frame_id = 'base_link' traj_msg.goal.trajectory.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] traj_msg.goal.trajectory.points = [start_config, goal_config] traj_msg.goal.goal_time_tolerance.secs = 0 traj_msg.goal.goal_time_tolerance.nsecs = 0 # rospy.loginfo(goal_config.positions[1]) # Message pubblication on "/joint_trajectory_action/goal" pub.publish(traj_msg) flag = False else: pass
def gotoBox(self): # hardcoded for now traj = JointTrajectory() traj.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] now = rospy.get_rostime() rospy.loginfo("Current time %i %i", now.secs, now.nsecs) traj.header.stamp = now rospy.loginfo("Going to Box") home = JointTrajectoryPoint() home.positions = self.home_position home.velocities = self.default_velocity home.time_from_start = rospy.Duration(10.0) traj.points.append(home) Bin_way1 = JointTrajectoryPoint() Bin_way1.positions = [ math.radians(4.78), self.home_position[1], self.home_position[2], self.home_position[3], self.home_position[4], self.home_position[5] ] Bin_way1.velocities = self.default_velocity Bin_way1.time_from_start = rospy.Duration(15.0) traj.points.append(Bin_way1) Bin_way2 = JointTrajectoryPoint() Bin_way2.positions = [ math.radians(4.78), math.radians(-78.95), math.radians(142.9), math.radians(-154.65), math.radians(271.99), math.radians(2.51) ] Bin_way2.velocities = self.default_velocity Bin_way2.time_from_start = rospy.Duration(20.0) traj.points.append(Bin_way2) Bin = JointTrajectoryPoint() Bin.positions = [ math.radians(33.62), math.radians(-35.03), math.radians(125.73), math.radians(-181.48), math.radians(275.56), math.radians(0.76) ] Bin.velocities = self.default_velocity Bin.time_from_start = rospy.Duration(25.0) traj.points.append(Bin) ag = FollowJointTrajectoryActionGoal() ag.goal.trajectory = traj self.joint_angle_pub.publish(ag) rospy.sleep(25)
def move(self, pos): msg = FollowJointTrajectoryActionGoal() msg.goal.trajectory.joint_names = self.joint_names point = JointTrajectoryPoint() point.positions = pos point.time_from_start = rospy.Duration(1.0 / 60.0) msg.goal.trajectory.points.append(point) self.action_server_client.send_goal(msg.goal) return True
def reset_move(self, pos): jtp_msg = JointTrajectory() self.jtp.publish(jtp_msg) msg = FollowJointTrajectoryActionGoal() msg.goal.trajectory.joint_names = self.joint_name_lst point = JointTrajectoryPoint() point.positions = pos point.time_from_start = rospy.Duration(0.0001) msg.goal.trajectory.points.append(point) self.jta.send_goal(msg.goal)
def __init__(self): self.trajPub = rospy.Publisher( '/arm_controller/follow_joint_trajectory/goal', FollowJointTrajectoryActionGoal, queue_size=10) self.pointPub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) self.msg2pub = FollowJointTrajectoryActionGoal() self.msg2 = JointTrajectory()
def arm_action_control(self, q_goal): print("== Waiting for action server... ") self.arm_client.wait_for_server() # JointTrajectory joint_goal = self.set_arm_joint_goal(q_goal) # FollowJointTrajectoryActionGoal action_goal = FollowJointTrajectoryActionGoal() action_goal.goal.trajectory = joint_goal # send goal self.arm_client.send_goal(action_goal.goal) self.arm_client.wait_for_result()
def arm_hand_action_control(self, q_arm_goal, q_hand_goal): print("== Waiting for action server... ") self.arm_client.wait_for_server() self.hand_client.wait_for_server() # JointTrajectory arm_joint_goal = self.set_arm_joint_goal(q_arm_goal) hand_joint_goal = self.set_arm_joint_goal(q_hand_goal) # FollowJointTrajectoryActionGoal arm_action_goal = FollowJointTrajectoryActionGoal() arm_action_goal.goal.trajectory = arm_joint_goal hand_action_goal = FollowJointTrajectoryActionGoal() hand_action_goal.goal.trajectory = hand_joint_goal # send goals self.arm_client.send_goal(arm_action_goal.goal) self.hand_client.send_goal(hand_action_goal.goal) # wait for results self.arm_client.wait_for_result() self.hand_client.wait_for_result()
def move_robot(pose): goal_pose = FollowJointTrajectoryActionGoal() goal_pose.goal.trajectory.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] goal_pose.goal.trajectory.points.append(JointTrajectoryPoint()) goal_pose.goal.trajectory.points[0].positions = pose goal_pose.goal.trajectory.points[0].velocities = [0, 0, 0, 0, 0, 0] goal_pose.goal.trajectory.points[ 0].time_from_start = rospy.Duration.from_sec(1.0) return goal_pose
def callback(self, anymsg, topic_name): if self.pause_button.isChecked(): return # In case of control_msgs/FollowJointTrajectoryActionGoal # set trajectory_msgs/JointTrajectory to 'msg' # Convert AnyMsg to trajectory_msgs/JointTrajectory msg_class = self.topic_name_class_map[topic_name] if msg_class == JointTrajectory: msg = JointTrajectory().deserialize(anymsg._buff) elif msg_class == FollowJointTrajectoryActionGoal: msg = FollowJointTrajectoryActionGoal().deserialize( anymsg._buff).goal.trajectory elif msg_class == DisplayTrajectory: if DisplayTrajectory().deserialize( anymsg._buff).trajectory.__len__() > 0: msg = DisplayTrajectory().deserialize( anymsg._buff).trajectory.pop().joint_trajectory else: rospy.logwarn( "Received planned trajectory has no waypoints in it. Nothing to plot!" ) return else: rospy.logerr('Wrong message type %s' % msg_class) return self.time = np.array([0.0] * len(msg.points)) (self.dis, self.vel, self.acc, self.eff) = ({}, {}, {}, {}) for joint_name in msg.joint_names: self.dis[joint_name] = np.array([0.0] * len(msg.points)) self.vel[joint_name] = np.array([0.0] * len(msg.points)) self.acc[joint_name] = np.array([0.0] * len(msg.points)) self.eff[joint_name] = np.array([0.0] * len(msg.points)) for i in range(len(msg.points)): point = msg.points[i] self.time[i] = point.time_from_start.to_sec() for j in range(len(msg.joint_names)): joint_name = msg.joint_names[j] if point.positions: self.dis[joint_name][i] = point.positions[j] if point.velocities: self.vel[joint_name][i] = point.velocities[j] if point.accelerations: self.acc[joint_name][i] = point.accelerations[j] if point.effort: self.eff[joint_name][i] = point.effort[j] if self.joint_names != msg.joint_names: self.joint_names = msg.joint_names self.refresh_tree() self.joint_names = msg.joint_names self.plot_graph()
def functional(): pub_gary = rospy.Publisher('/gary/joint_trajectory_action/goal', FollowJointTrajectoryActionGoal, queue_size=10) pub_rosey = rospy.Publisher('/rosey/joint_trajectory_action/goal', FollowJointTrajectoryActionGoal, queue_size=10) rospy.init_node('traj_maker', anonymous=True) time.sleep(4) rate = rospy.Rate(0.01) while not rospy.is_shutdown(): traj_waypoint_1_gary = JointTrajectoryPoint() traj_waypoint_1_rosey = JointTrajectoryPoint() traj_waypoint_1_gary.positions = [0, 0, 0, 0, 0, 0] traj_waypoint_1_rosey.positions = [0, 0, 0, 0, 0, 0] traj_waypoint_1_rosey.time_from_start.secs = 10 traj_waypoint_1_gary.time_from_start.secs = 10 # making message message_gary = FollowJointTrajectoryActionGoal() message_rosey = FollowJointTrajectoryActionGoal() # required headers header_gary = std_msgs.msg.Header(stamp=rospy.Time.now()) header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now()) message_gary.goal.trajectory.header = header_gary message_rosey.goal.trajectory.header = header_rosey message_gary.header = header_gary message_rosey.header = header_rosey # adding in joints joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', \ 'joint_5', 'joint_6'] message_gary.goal.trajectory.joint_names = joint_names message_rosey.goal.trajectory.joint_names = joint_names # appending up to 100 points # ex. for i in enumerate(len(waypoints)): append(waypoints[i]) message_gary.goal.trajectory.points.append(traj_waypoint_1_gary) message_rosey.goal.trajectory.points.append(traj_waypoint_1_rosey) # publishing to ROS node pub_gary.publish(message_gary) pub_rosey.publish(message_rosey) rate.sleep() if rospy.is_shutdown(): break break
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 command(): global flag global actual_config flag = False rospy.init_node('set_to_zero', anonymous=True) rospy.Subscriber("/joint_states", JointState, state_callback) pub = rospy.Publisher('/joint_trajectory_action/goal', FollowJointTrajectoryActionGoal, queue_size=10) rospy.sleep(1) if flag: # Starting configuration initialization based on the joint state stored by state_callback start_config = JointTrajectoryPoint() start_config.positions = actual_config start_config.time_from_start.secs = 0 start_config.time_from_start.nsecs = 0 # Goal configuration initialization: default position (all zeros) goal_config = JointTrajectoryPoint() goal_config.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] goal_config.time_from_start.secs = 1 goal_config.time_from_start.nsecs = 63310604 # Message inizialization traj_msg = FollowJointTrajectoryActionGoal() traj_msg.goal.trajectory.header.seq = 0 traj_msg.goal.trajectory.header.stamp.nsecs = 0 traj_msg.goal.trajectory.header.stamp.secs = 0 traj_msg.goal.trajectory.header.frame_id = 'base_link' traj_msg.goal.trajectory.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ] traj_msg.goal.trajectory.points = [start_config, goal_config] traj_msg.goal.goal_time_tolerance.secs = 0 traj_msg.goal.goal_time_tolerance.nsecs = 0 # rospy.loginfo(goal_config.positions[1]) # Message pubblication on "/joint_trajectory_action/goal" pub.publish(traj_msg) flag = False else: pass
def main(): # pub = rospy.Publisher('/if_any', String, queue_size=10) traj_pub = rospy.Publisher('/arm_controller/follow_joint_trajectory/goal', FollowJointTrajectoryActionGoal, queue_size=10) rospy.init_node('generate_trajectory', anonymous=True) rospy.Subscriber("/joint_states", JointState, js_callback) rospy.sleep(0.5) while 'last_js' not in globals() and not rospy.is_shutdown(): pass joint_limits = np.array([[-math.pi, math.pi]] * 6) # joint_limits[0:2, :] = 0. vel_limit = 1.0 acc = 10.0 time_step = 0.1 traj = gen_traj(joint_limits=joint_limits, action_step=10, duration=10, acc_mod=10., vel_limit=0.5, time_step=0.01) goal = FollowJointTrajectoryActionGoal() goal.goal.trajectory = traj # goal.header.stamp = rospy.Time.now() # goal.goal_id.stamp = rospy.Time.now() # goal.goal_id.id = ''.join(random.choice(string.ascii_uppercase + string.digits) for _ in range(10)) # print(goal) import rosbag bag = rosbag.Bag('test.bag', 'w') try: bag.write('/follow_joint_trajectory/goal', goal) finally: bag.close() traj_pub.publish(goal)
def make_trajectory_goal_msg(joint_trajectory_plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id='/base_link'): ''' Converts a joint trajectory plan to a FollowJoinTrajectoryActionGoal message which is compatible with the topic /arm_N/arm_controller/follow_joint_trajectory/goal ''' # Goal ID Generator id_gen = actionlib.GoalIDGenerator() # create joint trajectory message jtg = FollowJointTrajectoryActionGoal(goal_id=id_gen.generate_ID()) # fill the header jtg.header.seq = seq jtg.header.stamp.secs = 0 #secs jtg.header.stamp.nsecs = 0 #nsecs jtg.header.frame_id = frame_id # specify the joint names jtg.goal.trajectory.joint_names = joint_trajectory_plan.joint_trajectory.joint_names # fill the trajectory points and computer constraint times njtp = len(joint_trajectory_plan.joint_trajectory.points) for ii in range(0, njtp): jtp = JointTrajectoryPoint() jtp = copy.deepcopy(joint_trajectory_plan.joint_trajectory.points[ii]) jtp.time_from_start = rospy.Duration().from_sec(secs + dt*(ii+1)) jtg.goal.trajectory.points.append(jtp) '''jtg.goal.goal_tolerance = JointTolerance() jtg.goal.goal_tolerance.name = "goaltol" jtg.goal.goal_tolerance.position = 0.02 jtg.goal.goal_tolerance.velocity = 0.5 jtg.goal.goal_tolerance.acceleration = 0.5 jtg.goal.path_tolerance = JointTolerance() jtg.goal.path_tolerance.name = "goaltol" jtg.goal.path_tolerance.position = 0.02 jtg.goal.path_tolerance.velocity = 0.5 jtg.goal.path_tolerance.acceleration = 0.5 jtg.goal.goal_time_tolerance = rospy.Duration() jtg.goal.goal_time_tolerance.set(5, 0)''' return jtg
def test_callback(data_input): global message message = data_input.actual.positions msg_list = list(message) #msg_list[0] = int(message[0].encode('hex'),16) #for i in #msg_list = int(message.encode('hex'),16) #print('============= Received image data.',message) rospy.loginfo('=====received data %r', msg_list[0]) timer = Timer() dt = 0.1 p.setup(timestep=dt) # 0.1ms pub = rospy.Publisher('/arm_controller/follow_joint_trajectory/goal', FollowJointTrajectoryActionGoal, queue_size=10) command = FollowJointTrajectoryActionGoal() command.header.stamp = rospy.Time.now() command.goal.trajectory.joint_names = ['elbow'] point = JointTrajectoryPoint() point.positions = [rate_command / 10] point.time_from_start = rospy.Duration(1) command.goal.trajectory.points.append(point) pub.publish(command) rospy.loginfo('=====send command %r', command.goal.trajectory.points[0]) print("now plotting the network---------------") rospy.loginfo('--------now plotting---------------') n_panels = sum(a.shape[1] for a in pop_1_data.segments[0].analogsignalarrays) + 2 plt.subplot(n_panels, 1, 1) plot_spiketrains(pop_1_data.segments[0]) panel = 3 for array in pop_1_data.segments[0].analogsignalarrays: for i in range(array.shape[1]): plt.subplot(n_panels, 1, panel) plot_signal(array, i, colour='bg'[panel % 2]) panel += 1 plt.xlabel("time (%s)" % array.times.units._dimensionality.string) plt.setp(plt.gca().get_xticklabels(), visible=True) #
def move_robot(self, current_state): if current_state in self.state_to_gripper_pose: print( f"Moving robot to the pose p={self.state_to_gripper_pose[current_state][0]}," f" q={self.state_to_gripper_pose[current_state][1]}") # set reference position of the gripper ( link) pose_ik = PoseStamped() pose_ik.header.frame_id = "base_link" pose_ik.header.stamp = rospy.Time.now() pose_ik.pose = self.pq_to_pose( *self.state_to_gripper_pose[current_state]) req = GetPositionIKRequest() req.ik_request.group_name = "manipulator" req.ik_request.robot_state = self.robot_commander.get_current_state( ) req.ik_request.avoid_collisions = True req.ik_request.ik_link_name = self.arm_move_group.get_end_effector_link( ) req.ik_request.pose_stamped = pose_ik ik_response = self.compute_ik_srv(req) if ik_response.error_code.val == 1: goal_pose = FollowJointTrajectoryActionGoal() # print (arm_move_group.get_joints()) goal_pose.goal.trajectory.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] goal_pose.goal.trajectory.points.append(JointTrajectoryPoint()) goal_pose.goal.trajectory.points[ 0].positions = ik_response.solution.joint_state.position goal_pose.goal.trajectory.points[0].velocities = [ 0, 0, 0, 0, 0, 0 ] goal_pose.goal.trajectory.points[ 0].time_from_start = rospy.Duration.from_sec(1.0) self.pub.publish(goal_pose) else: print('Could not find solution to inverse kinematic')
def execute_cb(self, goal): success = True #create action for arm arm_action = FollowJointTrajectoryAction() #create two seperate goals, one for base and one for arm base_fjtag = FollowJointTrajectoryActionGoal() arm_fjtag = FollowJointTrajectoryActionGoal() #set the base goal and name the virtual joints base_goal = FollowJointTrajectoryGoal() base_jt = JointTrajectory() base_jt.joint_names = ["virtual_x", "virtual_y", "virtual_theta"] #set the arm goal and name the arm joints arm_goal = FollowJointTrajectoryGoal() arm_jt = JointTrajectory() arm_jt.joint_names = ["arm_joint_1", "arm_joint_2", "arm_joint_3","arm_joint_4","arm_joint_5"] for pt in goal.trajectory.points: # Fetch the base position from Moveit plan 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.time_from_start = pt.time_from_start base_jt.points.append(pt_temp) pt_temp = JointTrajectoryPoint() #allocate the last five pt.position(for arm) elements to arm trajectory 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 + rospy.Duration(0.1) arm_jt.points.append(pt_temp) base_goal.trajectory = base_jt arm_goal.trajectory = arm_jt base_fjtag.goal = base_goal arm_fjtag.goal = arm_goal arm_action = arm_goal # check that preempt has not been requested by the client if self._as.is_preempt_requested(): rospy.loginfo("This action has been preempted") self._as.set_preempted() success = False self._as.set_cancelled() #if preempted, can do something here else: self.pub1.publish(base_fjtag) self.arm_client.send_goal_and_wait(arm_action) success = True # self.pub2.publish(arm_fjtag) if success: rospy.loginfo('Action Succeeded') self._as.set_succeeded()