[EE Admittance] Weight of the end effector hard coded

include/sot/talos_balance/admittance-controller-end-effector.hh

@@ -128,7 +128,7 @@ protected:
   bool m_removeWeight;
 
   /// Robot Util instance to get the sensor frame
-  RobotUtil *m_robot_util;
+  RobotUtilShrPtr m_robot_util;
   /// Pinocchio robot model
   pinocchio::Model m_model;
   /// Pinocchio robot data

python/sot_talos_balance/test/appli_admittance_end_effector.py

@@ -14,8 +14,8 @@ q =  [0., 0., 1.018213, 0., 0., 0.]                                  #Base
 q += [0., 0., -0.411354, 0.859395, -0.448041, -0.001708]             #Left Leg
 q += [0., 0., -0.411354, 0.859395, -0.448041, -0.001708]             #Right Leg
 q += [0.0 ,  0.006761]                                               #Chest
-q += [0.25847 ,  0.173046, -0.0002, -0.525366, 0., 0.,  0.1, -0.005] #Left Arm
-q += [-0.25847 , -0.0, 0.19  , -1.61, 0.,  0.,  0.1,-0.005]          #Right Arm
+q += [ 0.25847, 0.173046, -0.0002, -0.525366, 0., 0., 0.1, -0.005]   #Left Arm
+q += [-0.25847, -0.0, 0.19, -1.61, 0., 0., 0.1, -0.005]              #Right Arm
 q += [0.,  0.]                                                       #Head
 robot.device.set(q)
 

src/admittance-controller-end-effector.cpp

@@ -180,7 +180,13 @@ DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
 
   if (m_removeWeight)
   {
-    w_vForce(2) += m_mass * 9.80665 - 1.99325154291384;
+    double weight = -14.604817920170488;
+    // Eigen::Vector3d OC(0.02097597, -0.02460337, -0.00272215);
+    // Vector w_OC = sensorPlacement.rotation() * OC;
+    w_vForce(2) -= weight;
+    // w_forceDes(3) += w_OC(1) * weight;
+    // w_forceDes(4) += -w_OC(0) * weight;
+    // w_vForce(2) += m_mass * 9.80665 - 1.99325154291384;
   }
 
   s = w_vForce;