Open wangjiwang opened 4 years ago
Thanks for your reply. And I try to use arm_with_torso_controller/follow_joint_trajectory controller to execute a pre-defined trajectory. It did not work. I can not find the reason, can you help me?
#!/usr/bin/env python import sys import rospy import actionlib from control_msgs.msg import (FollowJointTrajectoryAction, FollowJointTrajectoryGoal) from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint arm_joint_names = ['l_wheel_joint', 'r_wheel_joint', 'torso_lift_joint', 'bellows_joint', 'head_pan_joint', 'head_tilt_joint','shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint', 'l_gripper_finger_joint', 'r_gripper_finger_joint'] arm_joint_positions = [2.3345511882210843, -0.634820929568157, 1.3874957609389449e-11, 0.006625115661578738, -0.006317351703414076, 0.6313382345293581, 1.318234031118565, 1.3956276493932007, -0.2089051625781302, 1.7008202010543583, -0.016423456036472217, 1.6468142324488904, -0.008023511866540822, 0.02927438379458224, 0.02988700000824869] velocity = [12.07760822395682, 12.531155056815715, 0.00551848394843501, -0.006297094941102944, -0.027727924188788666, -0.8389266373685278, -0.01006305278808928, 0.06190390257346417, 0.014009208799648233, 0.046633352775736804, -0.016161009170007727, 0.08276079812692123, -0.01060212434698555, -0.003579291965645062, 0.003610553717902847] effort = [8.85, 8.85, -450.0, -1.4943480513086147, 0.09047042943010608, -0.10530705118107508, -1.8543929008080047, 8.507774908689282, 1.273713991824661, 14.859113858920301, -7.935003515304931, -6.557514430844646, -1.7112672692034643, -9.663865221953854, -10.336134778046146] if __name__ == "__main__": rospy.init_node("prepare_simulated_robot") if rospy.get_param("robot/serial") != "ABCDEFGHIJKLMNOPQRSTUVWX": rospy.logerr("This script should not be run on a real robot") sys.exit(-1) rospy.loginfo("Waiting for arm_controller...") arm_client = actionlib.SimpleActionClient("arm_with_torso_controller/follow_joint_trajectory", FollowJointTrajectoryAction) arm_client.wait_for_server() rospy.loginfo("...connected.") trajectory = JointTrajectory() trajectory.joint_names = arm_joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = arm_joint_positions trajectory.points[0].velocities = velocity trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions) trajectory.points[0].effort = effort trajectory.points[0].time_from_start = rospy.Duration(1.0) arm_goal = FollowJointTrajectoryGoal() arm_goal.trajectory = trajectory arm_goal.goal_time_tolerance = rospy.Duration(0.0) rospy.loginfo("Setting positions...") arm_client.send_goal(arm_goal) arm_client.wait_for_result(rospy.Duration(6.0)) rospy.loginfo("...done")
Thanks for your reply. And I try to use arm_with_torso_controller/follow_joint_trajectory controller to execute a pre-defined trajectory. It did not work. I can not find the reason, can you help me?