Add ddp in talos-torque control and python scripts to use sot-torque-control and TSID for controlling the robot in position and torque
The scripts main_sim_com_vel.py and main_sim_com_torque.py allow to replicate the ex1 and 2 of the TSID jupyter notebooks in position and torque control in Gazebo with Talos (see the jupyter here).
It should be use with the sot-torque-control package, after the merging request 71.
The scripts use the entity simpleInverseDyn which implements 5 tasks in TSID:
- CoM
- Contacts 6D (one for each foot)
- Base orientation (named waist)
- Posture
Add also a doxygen documentation for launching the examples in simulation.
Merge request reports
Activity
1 # -*- coding: utf-8 -*- changed this line in version 2 of the diff
137 print "Gonna start SoT"; 138 sleep(1.0); 139 start_sot(); 140 141 142 # # # --- ROS PUBLISHER ---------------------------------------------------------- 143 144 robot.publisher = create_rospublish(robot, 'robot_publisher') 145 create_topic(robot.publisher, robot.joint_pos_selec_ddp, 'sout', 'joint_pos', robot=robot, data_type='vector') 146 # create_topic(robot.publisher, robot.torque_ctrl, 'u', 'torque_ctrl_u', robot=robot, data_type='vector') 147 create_topic(robot.publisher, robot.traj_gen, 'q', 'q_des', robot=robot, data_type='vector') 148 # # create_topic(robot.publisher, robot.ctrl_manager, 'u', 'manager_u', robot=robot, data_type='vector') 149 # # create_topic(robot.publisher, robot.ctrl_manager, 'u_safe', 'manager_u_safe', robot=robot, data_type='vector') 150 create_topic(robot.publisher, robot.ddp_ctrl, 'tau', 'ddp_ctrl_tau', robot=robot, data_type='vector') 151 152 # # # # --- ROS SUBSCRIBER changed this line in version 2 of the diff
14 15 # --- SET INITIAL CONFIGURATION ------------------------------------------------ 16 # TMP: overwrite halfSitting configuration to use SoT joint order 17 q = [0., 0., 1.018213, 0., 0., 0.] # Free flyer 18 q += [0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708] # legs 19 q += [0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708] # legs 20 q += [0.0, 0.006761] # Chest 21 q += [0.25847, 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, -0.005] # arms 22 q += [-0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005] # arms 23 q += [0., 0.] # Head 24 25 robot.halfSitting = q 26 27 # --- CREATE ENTITIES ---------------------------------------------------------- 28 29 robot.ctrl_manager = create_ctrl_manager(conf.control_manager, conf.motor_params, dt) 154 144 // q_sot[9 ]=q_urdf[34]; 155 145 // q_sot[10]=q_urdf[35]; 156 146 // q_sot[11]=q_urdf[36]; 157 return true; 158 } 159 160 bool config_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf) 161 { 162 assert(q_urdf.size()==N_JOINTS+7); 163 assert(q_sot.size()==N_JOINTS+6); 164 // ********* RPY to Quat ********* 147 return true; 148 } 149 150 bool config_sot_to_urdf(dg::sot::ConstRefVector q_sot, dg::sot::RefVector q_urdf) { 151 assert(q_urdf.size() == N_JOINTS + 7); -
This seems to be very similar to the https://github.com/stack-of-tasks/sot-core/blob/bf6998f1f76ad46c22aa1f350273fac484b3ae7d/src/tools/robot-utils.cpp#L329
Again fine for now but we SHOULD REALLY clean this up.
mentioned in commit 3c5461dd
I clean the files and remove useless ones (I don't know what to do about talos_common, for me we can remove it). I also fix ddp files, which were broken. I will make a new PR on ddp-actuator-solver and sot-torque-control to add the related codes.
Edited by Noelie Ramuzat
