[nd-trajectory-generator] add synchronous trajectories with force and com

python/dynamic_graph/sot/torque_control/create_entities_utils.py

@@ -312,7 +312,7 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
     plug(robot.com_traj_gen.ddx,                  ctrl.com_ref_acc);
 
     plug(robot.rf_force_traj_gen.x,               ctrl.f_ref_right_foot);
-    plug(robot.lf_force_traj_gen.x,               ctrl.l_ref_right_foot);
+    plug(robot.lf_force_traj_gen.x,               ctrl.f_ref_left_foot);
 
 
     # rather than giving to the controller the values of gear ratios and rotor inertias

python/dynamic_graph/sot/torque_control/main.py

@@ -6,7 +6,7 @@
 
 from dynamic_graph import plug
 from dynamic_graph.sot.torque_control.se3_trajectory_generator import SE3TrajectoryGenerator
-from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_switch, connect_synchronous_trajectories
+from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_switch, connect_synchronous_trajectories, create_force_traj_gen
 from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_generator, create_com_traj_gen, create_encoders
 from dynamic_graph.sot.torque_control.create_entities_utils import create_imu_offset_compensation, create_estimators, create_imu_filter
 from dynamic_graph.sot.torque_control.create_entities_utils import create_base_estimator, create_position_controller, create_torque_controller
@@ -64,8 +64,8 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
     
     robot.traj_gen        = create_trajectory_generator(robot.device, dt);
     robot.com_traj_gen    = create_com_traj_gen(conf.balance_ctrl, dt);
-    robot.rf_force_traj_gen  = create_force_traj_gen("rf_force_ref", conf.balance_ctrl.LF_FORCE_DES, dt);
-    robot.lf_force_traj_gen  = create_force_traj_gen("rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, dt);
+    robot.rf_force_traj_gen  = create_force_traj_gen("rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, dt);
+    robot.lf_force_traj_gen  = create_force_traj_gen("lf_force_ref", conf.balance_ctrl.LF_FORCE_DES, dt);
 
     robot.traj_sync       = create_trajectory_switch();
     robot.rf_traj_gen     = SE3TrajectoryGenerator("tg_rf");
@@ -81,8 +81,8 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
 
     connect_synchronous_trajectories(robot.traj_sync,
                                      [robot.com_traj_gen,
-                                      robot.rf_force_traj_gen, robot.lf_force_traj_gen,
-                                      robot.rf_traj_gen, robot.lf_traj_gen])
+                                      robot.rf_force_traj_gen, robot.lf_force_traj_gen])
+    #robot.rf_traj_gen, robot.lf_traj_gen])
 
     robot.pos_ctrl        = create_position_controller(robot, conf.pos_ctrl_gains, dt);
     robot.torque_ctrl     = create_torque_controller(robot, conf.joint_torque_controller, conf.motor_params, dt);

python/dynamic_graph/sot/torque_control/tests/test_balance_ctrl_openhrp.py

@@ -36,7 +36,7 @@ def get_sim_conf():
     conf.motor_params              = motor_params;
     return conf;
     
-def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=False, start_sot=True):
+def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=False, startSoT=True):
     # BUILD THE STANDARD GRAPH
     conf = get_sim_conf();
     robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf);
@@ -76,7 +76,7 @@ def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=Fals
         plug(robot.q.sout,              robot.inv_dyn.q);
         plug(robot.v.sout,              robot.inv_dyn.v);
     
-    if(start_sot):
+    if(startSoT):
         start_sot();    
         # RESET FORCE/TORQUE SENSOR OFFSET
         sleep(10*robot.timeStep);

scripts/python_commands.txt

@@ -37,8 +37,13 @@ create_topic(robot.ros, robot.device.accelerometer,       'acc');
 ent.ctrl_manager.setCtrlMode('all','torque')
 robot.base_estimator.set_imu_weight(0.0);
 
-robot.com_traj_gen.setSpline("/local/rbudhira/git/sot/sot-torque-control/data/traj_com_y_0.08/com_spline.curve", 1)
+robot.com_traj_gen.setSpline("${SOT_TORQUE_INSTALL_DIR}/data/traj_com_y_0.08/com_spline.curve", -1)
+robot.lf_force_traj_gen.setSpline("${SOT_TORQUE_INSTALL_DIR}/data/traj_com_y_0.08/lf_force.curve", -1)
+robot.rf_force_traj_gen.setSpline("${SOT_TORQUE_INSTALL_DIR}/data/traj_com_y_0.08/rf_force.curve", -1)
 robot.traj_sync.turnOn()
+
+create_topic(robot.ros, robot.inv_dyn.f_ref_right_foot, 'right_foot_force_des');
+create_topic(robot.ros, robot.inv_dyn.f_ref_left_foot, 'left_foot_force_des');
 sleep(5.0)
 robot.com_traj_gen.move(1, 0.05, 1.5)
 sleep(2.0*7)

src/nd-trajectory-generator.cpp

@@ -400,6 +400,12 @@ namespace dynamicgraph
         SEND_MSG("Spline set to "+filename+". Now checking initial position", MSG_TYPE_INFO);
         // check current configuration is not too far from initial configuration
         bool needToMoveToInitConf = false;
+        if(timeToInitConf == -1)
+        {
+          m_splineReady = true;
+          SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
+          return;
+        }
         const VectorXd& xInit = (*m_splineTrajGen)(0.0);
         assert(xInit.size() == m_n);
         for(unsigned int i=0; i<m_n; i++)