Fix python example and some minor bugs

Author: edantec

examples/go2_fulldynamics.py

@@ -65,13 +65,13 @@
     kinematics_limits=True,
     force_cone=False,
 )
-T = 100
+T = 50
 
 dynproblem = FullDynamicsOCP(problem_conf, model_handler)
 dynproblem.createProblem(model_handler.getReferenceState(), T, force_size, gravity[2], False)
 
-T_ds = 50
-T_ss = 10
+T_ds = 10
+T_ss = 30
 N_simu = int(0.01 / 0.001)
 mpc_conf = dict(
     ddpIteration=1,
@@ -113,11 +113,10 @@
     "RL_foot": False,
     "RR_foot": False,
 }
-contact_phases = [contact_phase_quadru] * int(T_ds / 2)
-contact_phases += [contact_phase_lift] * T_ss
+contact_phases = [contact_phase_quadru] * T_ds
+contact_phases += [contact_phase_lift_FL] * T_ss
 contact_phases += [contact_phase_quadru] * T_ds
-contact_phases += [contact_phase_lift] * T_ss
-contact_phases += [contact_phase_quadru] * int(T_ds / 2)
+contact_phases += [contact_phase_lift_FR] * T_ss
 
 """ contact_phases = [contact_phase_quadru] * int(T_ds / 2)
 contact_phases += [contact_phase_lift] * T_ss
@@ -144,7 +143,7 @@
 qp = IDSolver(id_conf, model_handler.getModel())
 
 """ Friction """
-fcompensation = FrictionCompensation(model_handler.getModel(), nu)
+fcompensation = FrictionCompensation(model_handler.getModel(), True)
 
 """ Initialize simulation"""
 device = BulletRobot(
@@ -277,10 +276,10 @@
         )
 
         qp_torque = qp.solved_torque.copy()
-        corrected_torque = fcompensation.computeFriction(x_interp[nq + 6:], qp_torque)
-        device.execute(corrected_torque)
+        fcompensation.computeFriction(x_interp[nq + 6:], qp_torque)
+        device.execute(qp_torque)
 
-        u_multibody.append(copy.deepcopy(corrected_torque))
+        u_multibody.append(copy.deepcopy(qp_torque))
         x_multibody.append(x_measured)
 
 force_FL = np.array(force_FL)

src/friction-compensation.cpp

@@ -12,11 +12,11 @@ namespace simple_mpc
     }
     else
     {
-      // Ignore root joint
+      // Ignore universe joint
       nu_ = model.njoints - 1;
     }
-    dry_friction_ = model.friction.head(nu_);
-    viscuous_friction_ = model.damping.head(nu_);
+    dry_friction_ = model.friction.tail(nu_);
+    viscuous_friction_ = model.damping.tail(nu_);
   }
 
   void FrictionCompensation::computeFriction(Eigen::Ref<const VectorXd> velocity, Eigen::Ref<VectorXd> torque)
@@ -29,12 +29,7 @@ namespace simple_mpc
 
   double FrictionCompensation::signFunction(double x)
   {
-    if (x > 0)
-      return 1.0;
-    else if (x == 0)
-      return 0.0;
-    else
-      return -1.0;
+    return (x > 0) - (x < 0);
   }
 
 } // namespace simple_mpc

tests/friction.cpp

@@ -15,14 +15,14 @@ BOOST_AUTO_TEST_CASE(dry_viscuous_friction)
   const std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf";
 
   pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model);
-  long nu = 12;
-  model.friction = Eigen::VectorXd::Constant(nu, .5);
-  model.damping = Eigen::VectorXd::Constant(nu, .05);
+  long nu = model.nv - 6;
+  model.friction.tail(nu) = Eigen::VectorXd::Constant(nu, .5);
+  model.damping.tail(nu) = Eigen::VectorXd::Constant(nu, .05);
 
   FrictionCompensation friction = FrictionCompensation(model);
 
-  BOOST_CHECK_EQUAL(model.friction, friction.dry_friction_);
-  BOOST_CHECK_EQUAL(model.damping, friction.viscuous_friction_);
+  BOOST_CHECK_EQUAL(model.friction.tail(nu), friction.dry_friction_);
+  BOOST_CHECK_EQUAL(model.damping.tail(nu), friction.viscuous_friction_);
 
   Eigen::VectorXd velocity = Eigen::VectorXd::Random(nu);
   Eigen::VectorXd torque = Eigen::VectorXd::Random(nu);
@@ -31,7 +31,7 @@ BOOST_AUTO_TEST_CASE(dry_viscuous_friction)
   for (long i = 0; i < nu; i++)
   {
     double sgn = (velocity[i] > 0) - (velocity[i] < 0);
-    ctorque[i] = torque[i] + model.friction[i] * sgn + model.damping[i] * velocity[i];
+    ctorque[i] = torque[i] + model.friction[i + 6] * sgn + model.damping[i + 6] * velocity[i];
   }
 
   friction.computeFriction(velocity, torque);