#################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= [[dynamixel_controllers/Tutorials/ConnectingToDynamixelBus|connecting to Dynamixel bus]] ## descriptive title for the tutorial ## title =Creating a dynamixel action client controller ## multi-line description to be displayed in search ## description = This tutorial describes how to create a joint controller with one or more Robotis Dynamixel motors. ## the next tutorial description (optional) ## next = ## links to next tutorial (optional) ## next.0.link= ## next.1.link= ## what level user is this tutorial for ## level= (BeginnerCategory, IntermediateCategory, AdvancedCategory) ## keywords = #################################### <> <> All files that are created in this tutorial should be saved into my_dynamixel_tutorial package which we have created in [[dynamixel_controllers/Tutorials/ConnectingToDynamixelBus|previous tutorial]]. {{{ roscd my_dynamixel_tutorial }}} == Step1: Create a client == {{{#!python block=client #!/usr/bin/env python import roslib roslib.load_manifest('my_dynamixel_tutorial') import rospy import actionlib from std_msgs.msg import Float64 import trajectory_msgs.msg import control_msgs.msg from trajectory_msgs.msg import JointTrajectoryPoint from control_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal, FollowJointTrajectoryAction, FollowJointTrajectoryGoal class Joint: def __init__(self, motor_name): #arm_name should be b_arm or f_arm self.name = motor_name self.jta = actionlib.SimpleActionClient('/'+self.name+'_controller/follow_joint_trajectory', FollowJointTrajectoryAction) rospy.loginfo('Waiting for joint trajectory action') self.jta.wait_for_server() rospy.loginfo('Found joint trajectory action!') def move_joint(self, angles): goal = FollowJointTrajectoryGoal() char = self.name[0] #either 'f' or 'b' goal.trajectory.joint_names = ['joint_1'+char, 'joint_2'+char,'joint_3'+char] point = JointTrajectoryPoint() point.positions = angles point.time_from_start = rospy.Duration(3) goal.trajectory.points.append(point) self.jta.send_goal_and_wait(goal) def main(): arm = Joint('f_arm') arm.move_joint([0.5,1.5,1.0]) arm.move_joint([6.28,3.14,6.28]) if __name__ == '__main__': rospy.init_node('joint_position_tester') main() }}} ==== The Code Explained ==== Now, let's break down the code piece by piece. <> Library used for send a trajectory point <> Remember to add the dependecies to your manifest <> Library with action messages <> Create a client through the topic namespace/name_of_server.In this case the namespace is self.name wich would be b_arm f_arm depends on witch arm do you wanna move, so remember to create a meta controller for b_arms and for f_arms, you can check this at [[dynamixel_controllers/Tutorials/Creatingmetacontroller|previous tutorial]]. <> Get the namespace first character that should be b or f and then add this to the trajectory joint names, that should match with joint names in your configuration file (config.yaml), you can review at [[dynamixel_controllers/Tutorials/Creatingmetacontroller|previous tutorial]]. <> send 0.5,1.5,1.0 positions for respectives joints joint_1x, joint_2x, joint_3x == Step2: Executing a client == After you create the client you should launch the controllers and spawners created in [[http://wiki.ros.org/dynamixel_controllers/Tutorials/Creatingmetacontroller|previous tutorial]]. {{{ roslaunch my_dynamixel_tutorial start_meta_controller.launch }}} and then execute the client {{{ roscd my_dynamixel_tutorial python trajectory_client.py }}} now see motors move in a trajectory!!