1 # Whole body motion {#tutorial_whole_body_motion}
3 \section tutorial_snd_introduction Introduction
5 This tutorial gives a brief high level view of a complete whole body motion with a complex behavior using several components of the SoT.
9 1. the stack of tasks has been installed using the installation instruction provided [there](download);
10 2. you understood the [dynamic graph mechanism](a_dynamic_graph).
12 This tutorial details some examples to test the stack of tasks framework on the humanoid robot TALOS.
14 \subsection tutorial_snd_introduction_melodic For Melodic
16 To visualize the robot, you need to have ROS and the following packages for ROS Melodic:
18 sudo apt-get install ros-melodic-twist-mux ros-melodic-joy-teleop ros-melodic-moveit-ros-move-group ros-melodic-humanoid-nav-msgs ros-melodic-play-motion ros-melodic-ompl ros-melodic-moveit-planners-ompl ros-melodic-moveit-simple-controller-manager
21 \subsection tutorial_snd_introduction_kinetic For Kinetic
23 To visualize the robot, you need to have ROS and the following packages for ROS Kinetic:
25 sudo apt-get install ros-kinetic-twist-mux ros-kinetic-joy-teleop ros-kinetic-moveit-ros-move-group ros-kinetic-humanoid-nav-msgs ros-kinetic-play-motion ros-kinetic-ompl ros-kinetic-moveit-planners-ompl ros-kinetic-moveit-simple-controller-manager
27 You also need the following robotpkg binaries:
29 sudo apt-get install robotpkg-talos-dev
31 Make sure that [your environment is properly setup](d_setting_up_environment) and that the environments variables of your terminal contains "/opt/openrobots"
33 \section tutorial_snd_whole_body_motion Scripted whole body motion
37 In the following three scripts are used to generate a whole body motion.
38 One starting the Gazebo simulation, the second to start the general SoT framework,
39 the third to generate the control graph and the subsequent whole body motion.
41 ### Start the simulation
44 roslaunch talos_gazebo talos_gazebo.launch
46 WARNING: The first time you are launching this command it might take some time because gazebo is downloading several models from Internet.
48 ### Start the SoT-ROS interface for TALOS in simulation (Gazebo)
50 roslaunch roscontrol_sot_talos sot_talos_controller_gazebo.launch
52 ### Start the motion of the robot
54 cd /opt/openrobots/share/sot-talos/tests/
57 \section tutorial_snd_detailed_explanations Explanations
59 ### Detailed explanation of test.py
61 This script is run by a python interpreter outside the real-time control loop of the robot.
68 The first lines are simply import system modules and the ros python interface.
71 from std_srvs.srv import *
73 It is used to test if the services provided by the SoT-ROS interface are available.
76 from dynamic_graph_bridge.srv import *
77 from dynamic_graph_bridge_msgs.srv import *
79 Import the helper objects to access services provided by the SoT-ROS interface.
82 def launchScript(code,title,description = ""):
83 raw_input(title+': '+description)
87 if line != '' and line[0] != '#':
89 answer = runCommandClient(str(line))
90 rospy.logdebug(answer)
92 rospy.loginfo("...done with "+title)
94 This script is loading a python file which will be send to the embedded python interpreter of the SoT.
95 It is waiting the user to hit the enter key after display the python file to be loaded.
98 # Waiting for services
100 rospy.loginfo("Waiting for run_command")
101 rospy.wait_for_service('/run_command')
102 rospy.loginfo("...ok")
104 rospy.loginfo("Waiting for start_dynamic_graph")
105 rospy.wait_for_service('/start_dynamic_graph')
106 rospy.loginfo("...ok")
108 This part of the script waits that the services provided by SoT-ROS interface become available:
110 <li> <b>run_command which</b> is the service to send python commands.</li>
111 <li> <b>start_dynamic_graph</b> is the service to start the control of the robot.</li>
116 runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
117 runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
119 Two helper objects are created to interact with the services.
122 initCode = open( "appli.py", "r").read().split("\n")
124 The file **apply.py** explained below is loaded in the variable initCode.
125 It basically the control graph that will be applied to the robot.
128 rospy.loginfo("Stack of Tasks launched")
130 launchScript(initCode,'initialize SoT')
132 The last line is sending the script appli.py to the interpreter on the robot.
135 raw_input("Wait before starting the dynamic graph")
137 This line prints the string and waits for the user to hit enter
140 runCommandStartDynamicGraph()
142 The last line starts to apply the control law to the robot and evaluate the whole SoT control graph.
145 raw_input("Wait before moving the hand")
147 This line prints the string and waits for the user to hit enter
150 runCommandClient("target = (0.5,-0.2,1.0)")
151 runCommandClient("gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))")
152 runCommandClient("sot.push(taskRH.task.name)")
154 The first **runCommandClient** specifies a target position in (X,Y,Z) coordinates.
155 The second **runCommandClient** specifies the gains to apply to the task **taskRH** and the axis to control.
156 Here '111' means that all axis are controlled.
157 The last runCommandClient push the task **taskRH** in the solver.
160 except rospy.ServiceException, e:
161 rospy.logerr("Service call failed: %s" % e)
163 The last two lines deals with exception which might raise during the process.
165 ### Detailed explanations of appli.py
168 from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
169 from dynamic_graph.sot.core.matrix_util import matrixToTuple
170 from numpy import eye
172 The first line import object collecting objects to realize generic kinematic tasks.
175 taskRH = MetaTaskKine6d('rh',robot.dynamic,'rh',robot.OperationalPointsMap['right-wrist'])
176 handMgrip = eye(4); handMgrip[0:3,3] = (0.1,0,0)
177 taskRH.opmodif = matrixToTuple(handMgrip)
178 taskRH.feature.frame('desired')
180 The first line create a task **rh** at the operational point 'right-wrist'.
181 The second line creates a homogeneous matrix **handMgrip** . The rotational part is set to identity and the translation part
182 is set to **(0.1,0.0,0.0)**.
183 The third line set a modification of the operational point. It is such that the controlled frame is 'right-wrist'
184 multiplied at the left by **handMgrip**.
187 # --- STATIC COM (if not walking)
188 taskCom = MetaTaskKineCom(robot.dynamic)
189 robot.dynamic.com.recompute(0)
190 taskCom.featureDes.errorIN.value = robot.dynamic.com.value
191 taskCom.task.controlGain.value = 10
193 This task controls the CoM of the robot by reading the output of the entity **robot.dynamic**.
194 The second line initialize the output value of signal **robot.dynamic.com**.
195 It becomes the desired value in the third line.
196 The control gain is set to 10 in the fourth line.
200 #define contactLF and contactRF
201 contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
202 contactLF.feature.frame('desired')
203 contactLF.gain.setConstant(10)
205 locals()['contactLF'] = contactLF
207 The first line create a set of object necessary to maintain contact with the left-ankle.
208 The second line specifies the name of the desired feature.
209 The third line specifies the gain of the contact.
210 The fourth line maintains the position as a constraint.
213 contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
214 contactRF.feature.frame('desired')
215 contactRF.gain.setConstant(10)
217 locals()['contactRF'] = contactRF
219 The lines specified here are the same than for the previous contact.
222 from dynamic_graph import plug
223 from dynamic_graph.sot.core import SOT
225 The first line import bindings to the lower C++ framework.
228 sot.setSize(robot.dynamic.getDimension())
229 plug(sot.control,robot.device.control)
231 The first line instantiates a solver to generate a kinematic solver.
232 The second line defines the size of free variables.
233 The third line links the solution of the solver to the input of the robot.
236 from dynamic_graph.ros import RosPublish
237 ros_publish_state = RosPublish ("ros_publish_state")
238 ros_publish_state.add ("vector", "state", "/sot_control/state")
240 The first line import the python module to publish data in the ROS world (aka topics).
241 The second line instantiate the object to make the interface from the Stack-Of-Tasks world
242 to the ROS world. It creates an entity called "ros_publish_state".
244 The third line adds a topic called **/sot_control/state** from the signal **state**
245 of the entity **ros_publish_state**
247 plug (robot.device.state, ros_publish_state.state)
248 robot.device.after.addDownsampledSignal ("ros_publish_state.trigger", 100)
250 The first line connect the signal **robot.device.state** to the the signal **state** of the entity **ros_publish_state**.
251 The second line calls the signal **ros_publish_state.trigger** at 100 Hz after evaluating the control law.
253 sot.push(contactRF.task.name)
254 sot.push(contactLF.task.name)
255 sot.push(taskCom.task.name)
256 robot.device.control.recompute(0)
258 The first line push the right foot contact task at the top of the SoT.
259 The second line push the left foot contact task in the SoT.
260 The third line push the CoM task in the SoT.
261 The last line ask for a re-computation of the signal named **control** which belongs to the entity **device**.