stack-of-tasks · NoelieRamuzat · Apr 6, 2020 · May 5, 2020 · May 12, 2020 · May 12, 2020
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -40,6 +40,7 @@ ENDIF(INITIALIZE_WITH_NAN)
 
 # Project dependencies
 ADD_PROJECT_DEPENDENCY(sot-torque-control REQUIRED PKG_CONFIG_REQUIRES sot-torque-control)
+ADD_PROJECT_DEPENDENCY(dynamic_graph_bridge_msgs REQUIRED)
 
 IF(BUILD_PYTHON_INTERFACE)
   FINDPYTHON()
@@ -78,3 +79,31 @@ 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/sim_com.py
+                      script/sim_posture.py
+                      script/sim_sinusoid_elbow.py
+                      script/sim_wrist_force.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
+                      python/start_talos_block_gazebo.py
+                      DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+FOREACH(dir python launch worlds)
+  INSTALL(DIRECTORY ${dir}
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+ENDFOREACH(dir)
diff --git a/README.md b/README.md
@@ -1 +1,14 @@
-Implements on HRP-2 the sot-torque-control capabilities.
+This package implements torque control on the TALOS humanoid robot of the LAAS-CNRS: Pyrene.
+It uses the following packages:
+
+* [tsid](https://github.com/stack-of-tasks/tsid): The Task Space Inverse Dynamics library 
+* [sot-torque-control](https://github.com/stack-of-tasks/sot-torque-control): The wrapper of TSID in the Stack-of-Task framework
+
+One can find the documentation of the package in the ```/doc``` directory (doxygen documentation that is built when calling ```make install```) :
+* [The overview of the package](doc/Overview.md)
+* [The installation procedure](doc/installation.md)
+* [Instructions for running a simulation of Pyrene executing a CoM sinusoid in position or torque control](doc/running.md)
+* [Instructions for running a simulation or an experiment using the DDP on the right elbow of Pyrene](doc/ddpRun.md)
+* [Instructions for running a simulation of Pyrene executing a foot sinusoid in the air in torque control](doc/bellStepRun.md)
+* [Instructions for running a simulation of Pyrene walking in torque control](doc/walkRun.md)
+* [Instructions for running a simulation of Pyrene realizing a contact-force task in torque control with passivity](doc/forceEnergy.md)
diff --git a/doc/Overview.md b/doc/Overview.md
@@ -54,6 +54,12 @@ 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>.
+
+Instructions for running a simulation of Pyrene realizing a contact-force task in torque control with passivity can be found <a href="md_doc_forceEnergy.html">here</a>.
diff --git a/doc/bellStepRun.md b/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).
diff --git a/doc/forceEnergy.md b/doc/forceEnergy.md
@@ -0,0 +1,136 @@
+# Pyrene realizing a contact-force task in torque control with passivity
+
+In this simulation the robot creates a contact between a cylinder, hold in its left hand, and a block. The robot applies a 30N force along the z-axis on it before the block is removed. This set-up is meant to represent a first step toward complex operations with contacts. It exposes the problem of unexpected broken contact that can be due to slippage or disturbances.
+
+In the following, we demonstrate how to run this simulation with  <a href="https://github.com/stack-of-tasks/sot-torque-control">sot-torque-control</a>, <a href="https://github.com/loco-3d/sot-talos-balance">sot-talos-balance</a> and talos-torque-control.
+
+## Installation procedure
+
+The installation procedure is different than for the other simulations because it requires specific branches on some packages:
+
+* [sot-talos-balance](https://github.com/NoelieRamuzat/sot-talos-balance/tree/topic/fix_ctrl_manager): branch topic/fix_ctrl_manager
+* [tsid](https://github.com/NoelieRamuzat/tsid/tree/topic/task_energy_contact): topic/task_energy_contact
+* [sot-torque-control](https://github.com/NoelieRamuzat/sot-torque-control/tree/topic/task_contact_force_energy): branch topic/task_contact_force_energy
+
+You will need to clone the previous packages, switch to the appropriate branches and build them as explained in the <a href="md_doc_installation.html">installation page</a>.
+And on [talos-torque-control](https://github.com/NoelieRamuzat/talos-torque-control/tree/topic/task_energy) you will need to switch to the branch topic/task_energy:
+
+```
+git checkout topic/task_energy
+cd _build-RELEASE
+make install
+```
+
+
+## Start the simulation
+
+Go into the talos-torque-control package once successfully installed.
+```
+cd talos-torque-control
+```
+
+Start the simulation with the robot in the half-sitting position, a cylinder tool in its left gripper and a block:
+```
+python talos-torque-control/python/start_talos_block_gazebo.py
+```
+
+## Start the SoT in torque mode
+
+To start the SoT in simulation in torque mode, open a new terminal, then: 
+```
+roslaunch roscontrol_sot_talos sot_talos_controller_gazebo_effort.launch
+```
+
+## Run the test
+
+First of all, you need to go to the folder where the script is, assuming you are in the root directory (open a new terminal):
+
+```
+cd script
+```
+
+Then, you can just run the test for the contact-force task in torque control with passivity:
+
+```
+python sim_wrist_force.py
+```
+
+This will launch the simulation, some information will appear in the terminal following the beginning of the simulation.
+
+```
+Starting script whith inverse_dyn_balance_controller main_sim_wrist_force.py
+initialize SoT:
+```
+
+Press the "enter" key to continue.
+```
+command: robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl,conf.motor_params, dt, controlType="torque")
+standardoutput: WARNING: Could not connect dv_des from BalanceController to ForceTorqueEstimator
+WARNING: Could not connect rf/lf_force_traj_gen to f_ref_right/left_foot
+```
+
+Do not pay attention to the warning, it is a normal procedure.
+
+```
+Wait before starting the dynamic graph
+```
+Press the "enter" key to continue. The controller will start after that and the robot will lower its CoM.
+```
+Waiting before writing the graph
+```
+Press the "enter" key to continue. This command save the entity graph built by the controller.
+```
+WriteGraph in /tmp/sot_talos_tsid.dot
+Convert graph to PDF in /tmp/sot_talos_tsid_walk.pdf
+```
+It then convert the graph to PDF in the /tmp directory.
+
+```
+Wait before going to contact pose
+```
+Press the "enter" key to continue. The robot will put its left arm in position on the top of the block to be prepared to create a contact.
+
+```
+Calibrate wrist force sensors? [y/N] 
+Wait before running the calibration
+```
+Press the "y" and then the "enter" keys to calibrate the sensor.
+
+```
+Calibrating sensors...
+Sensors are calibrated!
+Wait before adding EnergyTask
+```
+Press the "enter" key to continue. It add the energy tank and constraint to the controller.  
+The controller scheme is now the following (if not displayed correctly see [here](pictures/torque_scheme_energy.png)):
+
+\image html torque_scheme_energy.png 
+
+```
+Wait before adding TaskLeftHandContact
+```
+Press the "enter" key to continue. The robot will create the contact between the tool and the block and try to maintain 0N force (thus there are some oscillations).
+
+```
+Wait before setting 10N force
+```
+Press the "enter" key to continue. The robot will then apply the desired force following 2 stages:
+
+* The desired force is first set to 10N to stabilize the contact: there is some slippage between the cylinder and the block until the force applied on the block is enough
+
+* Then it is increased to 30N to have a meaningful force application
+
+Finally after a delay of 15s the following message appears:
+
+```
+REMOVE BLOCK ! 
+```
+
+You will have to clic on the block in gazebo and the press the "Del" key to delete it (the block is removed). 
+
+Without the passivity constraint on the system, the robot quickly falls after the removal of the block. Whereas with the implemented solution, the force task in the controller is penalized by the energy tank and thus the tracking highly deteriorated, leading to a slow motion of the hand toward the ground. Thus, the controller has the time to remove the dangerous task, ensuring the safety of the system.
+
+
+## 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).
diff --git a/doc/installation.md b/doc/installation.md
@@ -2,7 +2,7 @@
 
 1. Clone the git repository:
 ```
-git clone --recursive [email protected]:pyrene-dev/talos-torque-control.git
+git clone --recursive https://github.com/stack-of-tasks/talos-torque-control.git
 cd talos-torque-control
 ```
 

diff --git a/doc/pictures/Sot_torque_control_framework_for_position_control.png b/doc/pictures/Sot_torque_control_framework_for_position_control.png
diff --git a/doc/pictures/Sot_torque_control_framework_for_torque_control.png b/doc/pictures/Sot_torque_control_framework_for_torque_control.png
diff --git a/doc/pictures/old/Sot_torque_control_framework_for_position_control.png b/doc/pictures/old/Sot_torque_control_framework_for_position_control.png
diff --git a/doc/pictures/old/Sot_torque_control_framework_for_torque_control.png b/doc/pictures/old/Sot_torque_control_framework_for_torque_control.png
diff --git a/doc/pictures/torque_scheme_energy.png b/doc/pictures/torque_scheme_energy.png
diff --git a/doc/running.md b/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.
 
@@ -56,6 +56,8 @@ It also saves the dynamic graph in /tmp/sot_talos_tsid_com.pdf.
 
 \image html Sot_torque_control_framework_for_position_control.png
 
+If not displayed correctly see [here](pictures/Sot_torque_control_framework_for_position_control.png)
+
 For torque control simply run:
 
 ```
@@ -65,6 +67,8 @@ python sim_com.py torque
 
 \image html Sot_torque_control_framework_for_torque_control.png
 
+If not displayed correctly see [here](pictures/Sot_torque_control_framework_for_torque_control.png)
+
 These two examples replicate the ex2 of the TSID jupyter notebooks in position and torque control in Gazebo (see the jupyter <a href="https://github.com/stack-of-tasks/tsid/blob/master/exercizes/notebooks/TSID_ex2.ipynb">here</a>). 
 
 

diff --git a/doc/walkRun.md b/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).
diff --git a/launch/simulation_block.launch b/launch/simulation_block.launch
@@ -0,0 +1,31 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="start_half_sitting" default="true"/>
+  <arg name="world_name" default="talos_block_full"/>
+  <arg name="world_filename" default="$(find talos-torque-control)/worlds/$(arg world_name).world"/>
+  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.16 -R 0.0 -P 0.0 -Y 0.0" unless="$(arg start_half_sitting)"/>
+  <arg name="gui" default="true"/>
+  <arg name="debug" default="false"/>
+  <arg name="recording" default="false"/>
+  <arg name="extra_gazebo_args" default=""/>
+
+  <arg name="load_model" default="true"/> <!-- AS: should probably be false by default -->
+  <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
+  <arg name="foot_collision" default="thinbox"/>
+  <arg name="enable_leg_passive" default="false"/>
+  <arg name="enable_fixed_robot" default="false"/>
+  <arg name="default_configuration_type" default="zeros"/>
+
+  <env name="GAZEBO_MODEL_PATH" value="$(find talos_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
+
+  <!-- start up world -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(arg world_filename)"/>
+    <arg name="gui" value="$(arg gui)"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="recording" value="$(arg recording)"/>
+    <arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
+  </include>
+
+</launch>
diff --git a/package.xml b/package.xml
@@ -11,5 +11,6 @@
   <author>Olivier Stasse</author>
   <maintainer email="[email protected]">Guilhem Saurel</maintainer>
 
+  <buildtool_depend>catkin</buildtool_depend>
   <depend>sot-torque-control</depend>
 </package>
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
@@ -23,6 +23,13 @@ SET(${PROJECT_NAME}_PYTHON
   main_ddp_talos.py
   main_sim_com_torque.py
   main_sim_com_vel.py
+  main_sim_posture_torque.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
+  main_sim_wrist_force.py
   )
 
 FOREACH(file ${${PROJECT_NAME}_PYTHON})