Add Angular Momentum task & use base-estimator entity to achieve walk in torque control

CMakeLists.txt

@@ -80,3 +80,26 @@ PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
 IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
   INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME})
 ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+
+# Install scripts
+find_package(catkin REQUIRED)
+
+catkin_package()
+catkin_install_python(PROGRAMS
+                      script/sim_walk_torque.py
+                      script/run_test_utils.py
+                      script/sim_walk_vel.py
+                      script/test_ddp_sinu_effort.py
+                      script/integ_sim_walk_torque.py
+                      script/integ_run_test_utils.py
+                      script/integ_sim_walk_vel.py
+                      python/dynamic_graph/sot/torque_control/talos/main_sim_walk_torque.py
+                      python/dynamic_graph/sot/torque_control/talos/main_sim_walk_torque_online.py
+                      python/dynamic_graph/sot/torque_control/talos/main_sim_walk_vel.py
+                      python/dynamic_graph/sot/torque_control/talos/main_sim_walk_vel_online.py
+                      DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+FOREACH(dir python traj_multicontact_api)
+  INSTALL(DIRECTORY ${dir}
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+ENDFOREACH(dir)

doc/Overview.md

@@ -54,6 +54,10 @@ Pay attention not to install ROS using robotpkg though, because it would install
 
 You can find the full installation procedure in the <a href="md_doc_installation.html">installation page</a>.
 
-Quick instructions on how to run a test can be found <a href="md_doc_running.html">here</a>.
+Instructions for running a simulation of Pyrene executing a CoM sinusoid in position or torque control can be found <a href="md_doc_running.html">here</a>.
 
 Instructions for running a simulation or an experiment using the DDP on the right elbow of Pyrene can be found <a href="md_doc_ddpRun.html">here</a>.
+
+Instructions for running a simulation of Pyrene executing a foot sinusoid in the air in torque control can be found <a href="md_doc_bellStepRun.html">here</a>.
+
+Instructions for running a simulation of Pyrene walking in torque control can be found <a href="md_doc_walkRun.html">here</a>.

doc/bellStepRun.md

@@ -0,0 +1,40 @@
+# Pyrene step in the air in torque control
+
+In the following, we demonstrate how to run the foot sinusoid simulation with  <a href="https://github.com/stack-of-tasks/sot-torque-control">sot-torque-control</a>, and talos-torque-control.
+
+## Start the simulation
+
+Start the simulation with the robot in the half-sitting position:
+```
+roslaunch talos_data talos_gazebo.launch start_half_sitting:=true
+```
+
+## Start the SoT in torque mode
+
+To start the SoT in simulation in torque mode: 
+```
+roslaunch roscontrol_sot_talos sot_talos_controller_gazebo_effort.launch
+```
+
+## Run the test
+
+First of all, you need to go to the folder where your script is.
+For running the ddp test of talos-torque-control, assuming you are in the root directory:
+
+```
+cd script
+```
+
+Then, you can just run the test :
+
+```
+python sim_torque_bellStep.py
+```
+
+This will launch the simulation making the robot executing a sinusoid movement of its left foot in the air (a "bell step") in torque control. 
+
+The script also saves the dynamic graph in /tmp/sot_talos_tsid_bellStep.pdf.
+
+## Other
+
+More information on how to use the SoT and how to work on Talos can be found <a href="https://wiki.laas.fr/robots/Pyrene">in the robot wiki page</a> (you need LAAS permissions to access this).

doc/installation.md

@@ -2,7 +2,7 @@
 
 1. Clone the git repository:
 ```
-git clone --recursive git@gitlab.laas.fr:pyrene-dev/talos-torque-control.git
+git clone --recursive https://github.com/stack-of-tasks/talos-torque-control.git
 cd talos-torque-control
 ```
 

doc/running.md

@@ -1,4 +1,4 @@
-# Running a test
+# Pyrene CoM sinusoid in position or torque control
 
 In the following, we quickly demonstrate how to run a test with  <a href="https://github.com/stack-of-tasks/sot-torque-control">sot-torque-control</a> and talos-torque-control.
 

doc/walkRun.md

@@ -0,0 +1,59 @@
+# Make Pyrene walk in torque control (quasistatic trajectories)
+
+In the following, we demonstrate how to run the walking simulation with  <a href="https://github.com/stack-of-tasks/sot-torque-control">sot-torque-control</a>, and talos-torque-control; using the reference quasistatic trajectories computed by <a href="https://github.com/loco-3d/multicontact-api">multicontact-api</a>.
+
+## Start the simulation
+
+Start the simulation with the robot in the half-sitting position:
+```
+roslaunch talos_data talos_gazebo.launch start_half_sitting:=true
+```
+
+## Start the SoT in torque mode
+
+To start the SoT in simulation in torque mode: 
+```
+roslaunch roscontrol_sot_talos sot_talos_controller_gazebo_effort.launch
+```
+
+## Run the test
+
+First of all, you need to go to the folder where your script is.
+For running the ddp test of talos-torque-control, assuming you are in the root directory:
+
+```
+cd script
+```
+
+Then, you can just run the test, specifying which type of walk you want the robot to execute (on spot or 20cm steps):
+
+```
+Usage: python sim_walk_torque.py walk_type:=[on_spot|walk_20] {path_folder_of_the_reference_trajectories}
+```
+For instance, for the walk on spot simulation, just run:
+
+```
+python sim_walk_torque.py on_spot
+```
+
+This will launch the simulation making the robot walk on spot in torque control (for now only a quasistatic movement). 
+
+The script also saves the dynamic graph in /tmp/sot_talos_tsid_walk.pdf.
+
+For the 20cm walk just specify "walk_20" instead of "on_spot" in the command line.
+
+If you have some reference trajectories which are not the ones of the talos-torque-control package, you can test them by specifying the absolute path of their folder:
+
+```
+python sim_walk_torque.py walk_20 path_to_folder_of_ref_trajectories
+```
+
+The trajectories must have a .dat extension and the following names:
+* am.dat -> angular momentum trajectory (3D vector to 9D vector if derivatives)
+* com.dat -> center of Mass trajectory (3D vector to 9D vector if derivatives)
+* leftFoot.dat and rightFoot.dat -> feet trajectories (12D SE3 vector to 36D SE3 vector if derivatives)
+* leftForceFoot.dat and rightForceFoot.dat -> feet forces trajectories (6D vector to 18D vector if derivatives)
+
+## Other
+
+More information on how to use the SoT and how to work on Talos can be found <a href="https://wiki.laas.fr/robots/Pyrene">in the robot wiki page</a> (you need LAAS permissions to access this).

package.xml

@@ -11,5 +11,6 @@
   <author>Olivier Stasse</author>
   <maintainer email="guilhem.saurel@laas.fr">Guilhem Saurel</maintainer>
 
+  <buildtool_depend>catkin</buildtool_depend>
   <depend>sot-torque-control</depend>
 </package>

python/CMakeLists.txt

@@ -23,6 +23,11 @@ SET(${PROJECT_NAME}_PYTHON
   main_ddp_talos.py
   main_sim_com_torque.py
   main_sim_com_vel.py
+  main_sim_walk_torque.py
+  main_sim_walk_torque_online.py
+  main_sim_walk_vel.py
+  main_sim_walk_vel_online.py
+  main_sim_torque_bellStep.py
   )
 
 FOREACH(file ${${PROJECT_NAME}_PYTHON})

python/dynamic_graph/sot/torque_control/talos/balance_ctrl_sim_conf.py

@@ -7,12 +7,15 @@
 
 # CONTROLLER GAINS
 NJ = 32
-kp_posture  = (10., 5., 5., 1., 1., 10., 10., 5., 5., 1., 1., 10., 500., 500., 50., 10., 10., 10., 10., 10., 10., 10., 50., 10., 10., 10., 10., 10., 10., 10., 100., 100.)  # proportional gain of postural task
+# kp_posture  = (10., 5., 5., 1., 1., 10., 10., 5., 5., 1., 1., 10., 5000., 5000., 50., 100., 10., 10., 10., 10., 100., 50., 50., 100., 10., 10., 10., 10., 100., 50., 100., 100.)  # proportional gain of postural task
+# kp_posture  = (50., 50.,50., 50., 50., 50., 50., 50., 50., 50., 50., 50., 500., 500., 50., 50., 50., 50., 50., 50., 50., 10., 50., 50., 50., 50., 50., 50., 50., 10., 100., 100.)  # proportional gain of postural task
+
+kp_posture  = (10., 5., 5., 1., 1., 10., 10., 5., 5., 1., 1., 10., 500., 500., 50., 10., 10., 10., 50., 10., 10., 10., 50., 10., 10., 10., 50., 10., 10., 10., 100., 100.)  # proportional gain of postural task
 kd_posture = tuple(2 * np.sqrt(kp_posture))
 
 kp_pos = np.array(
-(1300.,  1300.,  1300.,  1300.,  1300.,  1300.,
- 1300.,  1300.,  1300.,  1300.,  1300.,  1300.,
+(1300.,  1300.,  1300.,  1300.,  50., 50.,
+ 1300.,  1300.,  1300.,  1300.,  50., 50.,
  100., 100.,
  500.,  500.,  500.,  500.,  500.,  500., 500.,
  10.,
@@ -21,27 +24,43 @@
  10.,10.))
 
 kd_pos = np.array(
- (20.,  20.,  20.,  20.,  20.,  20.,
-  20.,  20.,  20.,  20.,  20.,  20.,
+ (20.,  20.,  20.,  20.,  0.,  0.,
+  20.,  20.,  20.,  20.,  0.,  0.,
   10.,  10.,
   5.,  5.,  5.,  5.,  5.,  5., 5.,   
   0.1,
   5.,  5.,  5.,  5.,  5.,  5., 5.,
   0.1,
   0.1, 0.1))
 
-kp_contact  = 30.0  # constraint proportional feedback gain
-kd_contact  = 2*sqrt(kp_contact)   # constraint derivative feedback gain
-kp_com      = 20.0
-kd_com      = 2*sqrt(kp_com)
+kp_tau = 0.1 * np.array(
+(5.0, 5.0,  5.0,  5.0,  8.5,  8.5,
+ 5.0, 5.0,  5.0,  5.0,  8.5,  8.5,
+ 5.0, 5.0,
+ 5.0, 5.0,  5.0,  5.0,  5.0,  0.0, 0.0,
+ 0.0,
+ 5.0, 5.0,  5.0,  5.0,  5.0,  0.0, 0.0,
+ 0.0,
+ 0.0, 0.0))
+
+kp_contact  = 30.0 # constraint proportional feedback gain
+kd_contact  = 2*sqrt(kp_contact) # constraint derivative feedback gain
+kp_com      = 100.0
+kd_com      = 2.0*sqrt(kp_com)
 kp_waist    = 500.0
 kd_waist    = 2.0*sqrt(kp_waist)
+kp_am       = 10.0
+kd_am       = 2.0*sqrt(kp_am)
+kp_feet     = 100.0
+kd_feet     = 2.0*sqrt(kp_feet)
 
 # # CONTROLLER WEIGTHS
-w_com  = 1.0
+w_com = 1.0
 w_posture = 1e-1 # weight of postural task
 w_forces = 1e-3
 w_waist = 1.0
+w_am = 2e-2
+w_feet = 1.0
 
 # CONTACT PARAMETERS
 RIGHT_FOOT_SIZES  = (0.1,  -0.11,  0.069,  -0.069) # pos x, neg x, pos y, neg y size 
@@ -60,5 +79,10 @@
 fMin = 1.0                       # minimum normal force
 fMax = 1e3                       # maximum normal force
 
-RF_FORCE_DES = (0, 0, 500, 0, 0, 0.)
-LF_FORCE_DES = (0, 0, 500, 0, 0, 0.)
+RF_FORCE_DES = (-1.046429493171104411e-04, -7.562765419402753819e-01, 4.353980355770353299e+02, 7.602554432679653473e-04, -2.207972285084590425e+00, -1.498280724396473806e-02)
+LF_FORCE_DES = (1.046429491392032729e-04, -7.562766302521330974e-01, 4.501708762222422138e+02, 7.602554434527064586e-04, -2.207972285084379038e+00, -1.498280720937920224e-02)
+# Gazebo
+# left_foot_pose_init:
+#(0.00018613902637534818, 0.08484247188164483, 0.10499893952050893, 0.9999993219959397, 1.5026205931086e-07, 0.0011644774099903166, 4.460331463820307e-07, 0.9999998688916376, -0.0005120708040054116, -0.001164477334262359, 0.0005120709762148502, 0.9999991908875991)
+# right_foot_pose_init:
+#(-0.00018606594667042176, -0.08484243593632501, 0.10496893511785998, 0.9999996580947196, 2.0881799427841137e-06, 0.0008269256817475812, -1.8919417197805814e-06, 0.9999999718398825, -0.00023731130305498609, -0.0008269261540099154, 0.0002373096574218008, 0.9999996299385627)

python/dynamic_graph/sot/torque_control/talos/control_manager_conf.py

@@ -5,13 +5,16 @@
 @author: adelpret
 """
 import numpy as np
+from rospkg import RosPack
+
+rospack = RosPack()
 
 NJ = 32;
 TAU_MAX                     = 1.*1e2;   # max joint torques (security check of ControlManager)
 CURRENT_MAX                 = 20.0;   # max motor current (security check of ControlManager)
 CTRL_MAX                    = 20.0;   # max desired current (security check of ControlManager)
-model_path                  = ["/opt/openrobots/share"];
-urdfFileName                = model_path[0] + "/talos_data/robots/talos_reduced.urdf";
+model_path = [rospack.get_path('talos_data') + "/../"]
+urdfFileName = rospack.get_path('talos_data') + "/urdf/talos_reduced_v2.urdf"
 ImuJointName                = "imu_joint";
 
 mapJointNameToID={