[nd-trajectory-generator] create setSpline to initialize spline. add trigger...

[nd-trajectory-generator] create setSpline to initialize spline. add trigger to play splines together.
create entity for trajectory synchronization

include/sot/torque_control/nd-trajectory-generator.hh

@@ -76,7 +76,8 @@ namespace dynamicgraph {
 
         /* --- SIGNALS --- */
         DECLARE_SIGNAL_IN(initial_value,  dynamicgraph::Vector);
-        DECLARE_SIGNAL(x,      OUT,       dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(trigger,        bool);
+        DECLARE_SIGNAL(x, OUT,            dynamicgraph::Vector);
         DECLARE_SIGNAL_OUT(dx,            dynamicgraph::Vector);
         DECLARE_SIGNAL_OUT(ddx,           dynamicgraph::Vector);
 
@@ -91,6 +92,9 @@ namespace dynamicgraph {
 
         void playSpline(const std::string& fileName);
 
+        void setSpline(const std::string& filename, const double& timeToInitConf);
+        void startSpline();
+        
         /** Print the current value of the specified component. */
         void getValue(const int& id);
 
@@ -169,6 +173,7 @@ namespace dynamicgraph {
         double            m_t;                /// current control loop time.
         unsigned int      m_n;                /// size of ouput vector
         unsigned int      m_iterLast;         /// last iter index
+        bool              m_splineReady;      /// true if the spline has been successfully loaded.
 
         std::vector<JTG_Status> m_status;     /// status of the component
         std::vector<parametriccurves::AbstractCurve<double, Eigen::Vector1d>* >  m_currentTrajGen;

python/dynamic_graph/sot/torque_control/create_entities_utils.py

@@ -5,6 +5,7 @@
 """
 
 from dynamic_graph import plug
+from dynamic_graph.sot.core.switch import Switch
 from dynamic_graph.sot.torque_control.force_torque_estimator import ForceTorqueEstimator
 from dynamic_graph.sot.torque_control.numerical_difference import NumericalDifference
 from dynamic_graph.sot.torque_control.joint_torque_controller import JointTorqueController
@@ -92,6 +93,20 @@ def create_com_traj_gen(conf, dt):
     com_traj_gen.init(dt,3);
     return com_traj_gen ;
 
+def create_force_traj_gen(name, initial_value, dt):
+    force_traj_gen = NdTrajectoryGenerator(name);
+    force_traj_gen.initial_value.value = initial_value;
+    force_traj_gen.init(dt,6);
+    return force_traj_gen ;
+
+def create_trajectory_switch():
+    traj_sync = Switch("traj_sync");
+    return traj_sync ;
+
+def connect_synchronous_trajectories(switch, list_of_traj_gens):
+  for traj_gen in list_of_traj_gens:
+    plug(switch.out, traj_gen.trigger);
+
 def create_free_flyer_locator(ent, robot_name="robot"):
     from dynamic_graph.sot.torque_control.free_flyer_locator import FreeFlyerLocator
     ff_locator = FreeFlyerLocator("ffLocator");
@@ -296,6 +311,10 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
     plug(robot.com_traj_gen.dx,                   ctrl.com_ref_vel);
     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);
+
+
     # rather than giving to the controller the values of gear ratios and rotor inertias
     # it is better to compute directly their product in python and pass the result
     # to the C++ entity, because otherwise we get a loss of precision

python/dynamic_graph/sot/torque_control/main.py

@@ -6,6 +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_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
@@ -63,6 +64,10 @@ 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.traj_sync       = create_trajectory_switch();
     robot.rf_traj_gen     = SE3TrajectoryGenerator("tg_rf");
     robot.lf_traj_gen     = SE3TrajectoryGenerator("tg_lf");
     robot.rf_traj_gen.init(dt);
@@ -74,6 +79,11 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
     robot.imu_filter                            = create_imu_filter(robot, dt);
     robot.base_estimator                        = create_base_estimator(robot, dt, conf.base_estimator);
 
+    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.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);
     robot.inv_dyn         = create_balance_controller(robot, conf.balance_ctrl,conf.motor_params, dt);

scripts/python_commands.txt

@@ -37,7 +37,8 @@ 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.playSpline("${SOTTORQUECONTROL_SOURCE_DIR}/data/traj_com_y_0.08/com_spline.curve")
+robot.com_traj_gen.setSpline("/local/rbudhira/git/sot/sot-torque-control/data/traj_com_y_0.08/com_spline.curve", 1)
+robot.traj_sync.turnOn()
 sleep(5.0)
 robot.com_traj_gen.move(1, 0.05, 1.5)
 sleep(2.0*7)

scripts/spline_trajectory_maker.py

@@ -8,7 +8,7 @@ LF_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/lf_force.curve"
 RH_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/rh_force.curve"
 LH_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/lh_force.curve"
 VERIFY=False
-WRITE_OUTPUT =True
+WRITE_OUTPUT =False
 #PLOT =True
 ####CONFIG######################
 

src/nd-trajectory-generator.cpp

@@ -50,10 +50,12 @@ namespace dynamicgraph
           NdTrajectoryGenerator(const std::string& name)
             : Entity(name)
             ,CONSTRUCT_SIGNAL_IN(initial_value,dynamicgraph::Vector)
-            ,CONSTRUCT_SIGNAL(x,   OUT, dynamicgraph::Vector)
+            ,CONSTRUCT_SIGNAL_IN(trigger,bool)
+            ,CONSTRUCT_SIGNAL(x, OUT,  dynamicgraph::Vector)
             ,CONSTRUCT_SIGNAL_OUT(dx,  dynamicgraph::Vector, m_xSOUT)
             ,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT)
             ,m_firstIter(true)
+            ,m_splineReady(false)
             ,m_initSucceeded(false)
             ,m_n(1)
             ,m_t(0)
@@ -61,7 +63,8 @@ namespace dynamicgraph
       {
         BIND_SIGNAL_TO_FUNCTION(x,   OUT, dynamicgraph::Vector);
 
-        Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN);
+        Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN 
+                                    <<m_triggerSIN);
 
         /* Commands. */
         addCommand("init",
@@ -87,10 +90,11 @@ namespace dynamicgraph
                                                     "(double) final value",
                                                     "(double) time to reach the final value in sec")));
 
-        addCommand("playSpline",
-                   makeCommandVoid1(*this, &NdTrajectoryGenerator::playSpline,
-                                    docCommandVoid1("Load serialized spline from file",
-                                                    "(string)   filename")));
+        addCommand("setSpline",
+                   makeCommandVoid2(*this, &NdTrajectoryGenerator::setSpline,
+                                    docCommandVoid2("Load serialized spline from file",
+                                                    "(string)   filename",
+                                                    "(double) time to initial conf")));
 
         /*        addCommand("startTriangle",
                    makeCommandVoid4(*this, &NdTrajectoryGenerator::startTriangle,
@@ -198,6 +202,7 @@ namespace dynamicgraph
           }
           else if(iter == m_iterLast)
           {
+            if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
             if(m_status[0]==JTG_TEXT_FILE)
             {
               s = (*m_textFileTrajGen)(m_t);
@@ -215,6 +220,7 @@ namespace dynamicgraph
           m_iterLast = iter;
           m_t += m_dt;
 
+          if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
           if(m_status[0]==JTG_TEXT_FILE)
           {
             if(!m_textFileTrajGen->checkRange(m_t))
@@ -241,7 +247,8 @@ namespace dynamicgraph
                 m_noTrajGen[i]->setInitialPoint(s(i));
                 m_status[i] = JTG_STOP;
               }
-              SEND_MSG("Spline trajectory ended.", MSG_TYPE_INFO);
+              m_splineReady =false;
+              SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
               m_t =0;
             }
             else
@@ -378,7 +385,7 @@ namespace dynamicgraph
         }
       }
 
-      void NdTrajectoryGenerator::playSpline(const std::string& filename)
+      void NdTrajectoryGenerator::setSpline(const std::string& filename, const double& timeToInitConf)
       {
         if(!m_initSucceeded)
           return SEND_MSG("Cannot start spline before initialization!",MSG_TYPE_ERROR);
@@ -390,9 +397,10 @@ namespace dynamicgraph
         if(!m_splineTrajGen->loadFromFile(filename))
           return SEND_MSG("Error while loading spline"+filename, MSG_TYPE_ERROR);
 
+        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;
-        const VectorXd& xInit = (*m_splineTrajGen)(0.002);
+        const VectorXd& xInit = (*m_splineTrajGen)(0.0);
         assert(xInit.size() == m_n);
         for(unsigned int i=0; i<m_n; i++)
           if(fabs(xInit[i] - (*m_currentTrajGen[i])(m_t)[0]) > 0.001)
@@ -410,7 +418,7 @@ namespace dynamicgraph
 
             m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
             m_minJerkTrajGen[i]->setFinalPoint(xInit[i]);
-            m_minJerkTrajGen[i]->setTimePeriod(4.0);
+            m_minJerkTrajGen[i]->setTimePeriod(timeToInitConf);
             m_status[i] = JTG_MIN_JERK;
             m_currentTrajGen[i] = m_minJerkTrajGen[i];
             SEND_MSG("MinimumJerk trajectory for index "+ toString(i) +" to go to final position" + toString(xInit[i]), MSG_TYPE_WARNING);
@@ -418,7 +426,13 @@ namespace dynamicgraph
           m_t = 0.0;
           return;
         }
+        m_splineReady = true;
+        SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);        
+      }
 
+      void NdTrajectoryGenerator::startSpline()
+      {
+        if(m_status[0]==JTG_SPLINE) return;
         m_t = 0.0;
         for(unsigned int i=0; i<m_n; i++)
         {
@@ -577,6 +591,7 @@ namespace dynamicgraph
         unsigned int i = id;
         m_noTrajGen[i]->setInitialPoint((*m_currentTrajGen[i])(m_t)[0]);
         m_status[i] = JTG_STOP;
+        m_splineReady = false;
         m_currentTrajGen[i] = m_noTrajGen[i];
         m_t = 0.0;
       }