Skip to content
Snippets Groups Projects
Commit f9cd29b5 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[distribute] CoP

parent 78cd90b4
No related branches found
No related tags found
No related merge requests found
Pipeline #4554 failed
Stage: test
Hide whitespace changes
Inline Side-by-side
include/sot/talos_balance/distribute-wrench.hh
+ 8
− 3
View file @ f9cd29b5
......@@ -76,9 +76,9 @@ namespace dynamicgraph {
DECLARE_SIGNAL_INNER(qp_computations, int);
DECLARE_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector);
// DECLARE_SIGNAL_OUT(copLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(copLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector);
// DECLARE_SIGNAL_OUT(copRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(copRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
......@@ -88,7 +88,7 @@ namespace dynamicgraph {
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
// dynamicgraph::Vector computeCoP(const dynamicgraph::Vector & wrench, const MatrixHomogeneous & pose) const;
dynamicgraph::Vector computeCoP(const dynamicgraph::Vector & wrench, const pinocchio::SE3 & pose) const;
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
......@@ -96,6 +96,11 @@ namespace dynamicgraph {
pinocchio::Data m_data; /// Pinocchio robot data
RobotUtilShrPtr m_robot_util;
pinocchio::SE3 m_ankle_M_ftSens; /// ankle to F/T sensor transformation
pinocchio::FrameIndex m_left_foot_id;
pinocchio::FrameIndex m_right_foot_id;
dynamicgraph::Vector m_wrenchLeft;
dynamicgraph::Vector m_wrenchRight;
......
src/distribute-wrench.cpp
+ 25
− 20
View file @ f9cd29b5
......@@ -16,6 +16,8 @@
#include "sot/talos_balance/distribute-wrench.hh"
#include <iostream>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/command-bind.h>
......@@ -45,8 +47,7 @@ namespace dynamicgraph
#define INNER_SIGNALS m_kinematics_computations << m_qp_computations
//#define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_copRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT
#define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_wrenchRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT
#define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_copRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
......@@ -66,9 +67,9 @@ namespace dynamicgraph
, CONSTRUCT_SIGNAL_INNER(kinematics_computations, int, m_qSIN)
, CONSTRUCT_SIGNAL_INNER(qp_computations, int, m_wrenchDesSIN << m_kinematics_computationsSINNER)
, CONSTRUCT_SIGNAL_OUT(wrenchLeft, dynamicgraph::Vector, m_qp_computationsSINNER)
// , CONSTRUCT_SIGNAL_OUT(copLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
, CONSTRUCT_SIGNAL_OUT(copLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
, CONSTRUCT_SIGNAL_OUT(wrenchRight, dynamicgraph::Vector, m_qp_computationsSINNER)
// , CONSTRUCT_SIGNAL_OUT(copRight, dynamicgraph::Vector, m_wrenchRightSOUT)
, CONSTRUCT_SIGNAL_OUT(copRight, dynamicgraph::Vector, m_wrenchRightSOUT)
, CONSTRUCT_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector, m_wrenchLeftSOUT << m_wrenchRightSOUT)
, CONSTRUCT_SIGNAL_OUT(zmpRef, dynamicgraph::Vector, m_wrenchRefSOUT)
, CONSTRUCT_SIGNAL_OUT(emergencyStop, bool, m_zmpRefSOUT)
......@@ -114,6 +115,14 @@ namespace dynamicgraph
return;
}
assert(m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
m_left_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
m_right_foot_id = m_model.getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
m_ankle_M_ftSens = pinocchio::SE3(Eigen::Matrix3d::Identity(), m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ.head<3>());
// TODO: initialize m_qp1
m_qp2.problem(12,6,0);
......@@ -121,11 +130,12 @@ namespace dynamicgraph
m_initSucceeded = true;
}
/*
dynamicgraph::Vector
DistributeWrench::computeCoP(const dg::Vector & wrench, const MatrixHomogeneous & pose) const
DistributeWrench::computeCoP(const dg::Vector & wrenchGlobal, const pinocchio::SE3 & pose) const
{
const double h = pose(2,3);
dg::Vector wrench = pose.actInv(pinocchio::Force(wrenchGlobal)).toVector();
const double h = pose.translation()[2];
const double fx = wrench[0];
const double fy = wrench[1];
......@@ -136,6 +146,7 @@ namespace dynamicgraph
double m_eps = 0.1; // temporary
double px, py;
if(fz >= m_eps/2)
{
px = (- ty - fx*h)/fz;
......@@ -146,18 +157,15 @@ namespace dynamicgraph
px = 0.0;
py = 0.0;
}
const double pz = - h;
dg::Vector copLocal(3);
copLocal[0] = px;
copLocal[1] = py;
copLocal[2] = pz;
const double pz = 0.0;
dg::Vector cop = pose.linear()*copLocal + pose.translation();
dg::Vector cop(3);
cop[0] = px;
cop[1] = py;
cop[2] = pz;
return cop;
}
*/
/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
......@@ -264,7 +272,6 @@ namespace dynamicgraph
return s;
}
/*
DEFINE_SIGNAL_OUT_FUNCTION(copLeft, dynamicgraph::Vector)
{
if(!m_initSucceeded)
......@@ -276,7 +283,7 @@ namespace dynamicgraph
const Eigen::VectorXd & wrenchLeft = m_wrenchLeftSOUT(iter);
// stub
const MatrixHomogeneous & pose = MatrixHomogeneous::Identity();
const pinocchio::SE3 pose = m_data.oMf[m_left_foot_id] * m_ankle_M_ftSens;
s = computeCoP(wrenchLeft,pose);
......@@ -293,14 +300,12 @@ namespace dynamicgraph
const Eigen::VectorXd & wrenchRight = m_wrenchRightSOUT(iter);
// stub
const MatrixHomogeneous & pose = MatrixHomogeneous::Identity();
const pinocchio::SE3 pose = m_data.oMf[m_right_foot_id] * m_ankle_M_ftSens;
s = computeCoP(wrenchRight,pose);
return s;
}
*/
DEFINE_SIGNAL_OUT_FUNCTION(wrenchRef, dynamicgraph::Vector)
{
......
unittest/python/test_distribute.py
+ 19
− 1
View file @ f9cd29b5
......@@ -50,12 +50,14 @@ rightPos = data.oMf[rightId]
#print( "%s: %d" % (rightName,rightId) )
#print(rightPos)
print( "wrenches in GLOBAL frame:" )
g = 9.81
fz = m*g
force = [0.0, 0.0, fz]
forceLeft = [0.0, 0.0, fz/2]
forceRight = [0.0, 0.0, fz/2]
lever = float(com[0] - rightPos.translation[0])
lever = float(com[0])
tauy = -fz*lever
wrench = force + [0.0, tauy, 0.0]
wrenchLeft = forceLeft + [0.0, tauy/2, 0.0]
......@@ -65,6 +67,14 @@ print( "desired wrench: %s" % str(wrench) )
print( "expected left wrench: %s" % str(wrenchLeft) )
print( "expected right wrench: %s" % str(wrenchRight) )
print( "CoP in LOCAL sole frame:" )
copLeft = [float(com[0] - leftPos.translation[0]), -float(leftPos.translation[1]), 0.]
copRight = [float(com[0] - rightPos.translation[0]), -float(rightPos.translation[1]), 0.]
print( "expected left CoP: %s" % str(copLeft) )
print( "expected right CoP: %s" % str(copRight) )
# --- Parameter server ---
print("--- Parameter server ---")
......@@ -89,6 +99,14 @@ assertApprox(wrenchLeft,distribute.wrenchLeft.value,6)
print( "resulting right wrench: %s" % str(distribute.wrenchRight.value) )
assertApprox(wrenchRight,distribute.wrenchRight.value,6)
distribute.copLeft.recompute(0)
distribute.copRight.recompute(0)
print( "resulting left CoP: %s" % str(distribute.copLeft.value) )
assertApprox(copLeft,distribute.copLeft.value,3)
print( "resulting right CoP: %s" % str(distribute.copRight.value) )
assertApprox(copRight,distribute.copRight.value,3)
distribute.emergencyStop.recompute(0)
stop = distribute.emergencyStop.value
np.testing.assert_equal(stop,0)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment