sot-doc  1.1.0
Documentation entry point for the Stack-Of-Tasks
doc/Tutorial_Whole_Body_Motion.md
1 # Whole body motion {#tutorial_whole_body_motion}
2 
3 \section tutorial_snd_introduction Introduction
4 
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.
6 
7 It is assumed that:
8 
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).
11 
12 This tutorial details some examples to test the stack of tasks framework on the humanoid robot TALOS.
13 
14 \subsection tutorial_snd_introduction_melodic For Melodic
15 
16 To visualize the robot, you need to have ROS and the following packages for ROS Melodic:
17 
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
19 
20 
21 \subsection tutorial_snd_introduction_kinetic For Kinetic
22 
23 To visualize the robot, you need to have ROS and the following packages for ROS Kinetic:
24 
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
26 
27 You also need the following robotpkg binaries:
28 
29  sudo apt-get install robotpkg-talos-dev
30 
31 Make sure that [your environment is properly setup](d_setting_up_environment) and that the environments variables of your terminal contains "/opt/openrobots"
32 
33 \section tutorial_snd_whole_body_motion Scripted whole body motion
34 
35 ### Introduction
36 
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.
40 
41 ### Start the simulation
42 
43 
44  roslaunch talos_gazebo talos_gazebo.launch
45 
46 WARNING: The first time you are launching this command it might take some time because gazebo is downloading several models from Internet.
47 
48 ### Start the SoT-ROS interface for TALOS in simulation (Gazebo)
49 
50  roslaunch roscontrol_sot_talos sot_talos_controller_gazebo.launch
51 
52 ### Start the motion of the robot
53 
54  cd /opt/openrobots/share/sot-talos/tests/
55  python test.py
56 
57 \section tutorial_snd_detailed_explanations Explanations
58 
59 ### Detailed explanation of test.py
60 
61 This script is run by a python interpreter outside the real-time control loop of the robot.
62 
63 
64  #!/usr/bin/python
65  import sys
66  import rospy
67 
68 The first lines are simply import system modules and the ros python interface.
69 
70 
71  from std_srvs.srv import *
72 
73 It is used to test if the services provided by the SoT-ROS interface are available.
74 
75 
76  from dynamic_graph_bridge.srv import *
77  from dynamic_graph_bridge_msgs.srv import *
78 
79 Import the helper objects to access services provided by the SoT-ROS interface.
80 
81 
82  def launchScript(code,title,description = ""):
83  raw_input(title+': '+description)
84  rospy.loginfo(title)
85  rospy.loginfo(code)
86  for line in code:
87  if line != '' and line[0] != '#':
88  print line
89  answer = runCommandClient(str(line))
90  rospy.logdebug(answer)
91  print answer
92  rospy.loginfo("...done with "+title)
93 
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.
96 
97 
98  # Waiting for services
99  try:
100  rospy.loginfo("Waiting for run_command")
101  rospy.wait_for_service('/run_command')
102  rospy.loginfo("...ok")
103 
104  rospy.loginfo("Waiting for start_dynamic_graph")
105  rospy.wait_for_service('/start_dynamic_graph')
106  rospy.loginfo("...ok")
107 
108 This part of the script waits that the services provided by SoT-ROS interface become available:
109 <ul>
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>
112 </ul>
113 
114 
115 
116  runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
117  runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
118 
119 Two helper objects are created to interact with the services.
120 
121 
122  initCode = open( "appli.py", "r").read().split("\n")
123 
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.
126 
127 
128  rospy.loginfo("Stack of Tasks launched")
129 
130  launchScript(initCode,'initialize SoT')
131 
132 The last line is sending the script appli.py to the interpreter on the robot.
133 
134 
135  raw_input("Wait before starting the dynamic graph")
136 
137 This line prints the string and waits for the user to hit enter
138 
139 
140  runCommandStartDynamicGraph()
141 
142 The last line starts to apply the control law to the robot and evaluate the whole SoT control graph.
143 
144 
145  raw_input("Wait before moving the hand")
146 
147 This line prints the string and waits for the user to hit enter
148 
149 
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)")
153 
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.
158 
159 
160  except rospy.ServiceException, e:
161  rospy.logerr("Service call failed: %s" % e)
162 
163 The last two lines deals with exception which might raise during the process.
164 
165 ### Detailed explanations of appli.py
166 
167 
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
171 
172 The first line import object collecting objects to realize generic kinematic tasks.
173 
174 
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')
179 
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**.
185 
186 
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
192 
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.
197 
198 
199  # --- CONTACTS
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)
204  contactLF.keep()
205  locals()['contactLF'] = contactLF
206 
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.
211 
212 
213  contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
214  contactRF.feature.frame('desired')
215  contactRF.gain.setConstant(10)
216  contactRF.keep()
217  locals()['contactRF'] = contactRF
218 
219 The lines specified here are the same than for the previous contact.
220 
221 
222  from dynamic_graph import plug
223  from dynamic_graph.sot.core import SOT
224 
225 The first line import bindings to the lower C++ framework.
226 
227  sot = SOT('sot')
228  sot.setSize(robot.dynamic.getDimension())
229  plug(sot.control,robot.device.control)
230 
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.
234 
235 
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")
239 
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".
243 
244 The third line adds a topic called **/sot_control/state** from the signal **state**
245 of the entity **ros_publish_state**
246 
247  plug (robot.device.state, ros_publish_state.state)
248  robot.device.after.addDownsampledSignal ("ros_publish_state.trigger", 100)
249 
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.
252 
253  sot.push(contactRF.task.name)
254  sot.push(contactLF.task.name)
255  sot.push(taskCom.task.name)
256  robot.device.control.recompute(0)
257 
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**.