Commit 9e44aacb authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Extend InverseDynamicsFormulationAccForce to work also for robot manipulators,...

Extend InverseDynamicsFormulationAccForce to work also for robot manipulators, which are fully-actuated, so they do not need to specify the unactuated base dynamics as equality constraints in the TSID optimization problem.
parent b705af7d
......@@ -159,6 +159,7 @@ namespace tsid
double m_t; /// time
unsigned int m_k; /// number of contact-force variables
unsigned int m_v; /// number of acceleration variables
unsigned int m_u; /// number of unactuated DoFs
unsigned int m_eq; /// number of equality constraints
unsigned int m_in; /// number of inequality constraints
Matrix m_Jc; /// contact force Jacobian
......
......@@ -44,13 +44,14 @@ InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std
bool verbose):
InverseDynamicsFormulationBase(name, robot, verbose),
m_data(robot.model()),
m_baseDynamics("base-dynamics", 6, robot.nv()),
m_baseDynamics("base-dynamics", robot.nv()-robot.na(), robot.nv()),
m_solutionDecoded(false)
{
m_t = 0.0;
m_v = robot.nv();
m_u = robot.nv() - robot.na();
m_k = 0;
m_eq = 6;
m_eq = m_u;
m_in = 0;
m_hqpData.resize(2);
m_Jc.setZero(m_k, m_v);
......@@ -82,7 +83,7 @@ unsigned int InverseDynamicsFormulationAccForce::nIn() const
void InverseDynamicsFormulationAccForce::resizeHqpData()
{
m_Jc.setZero(m_k, m_v);
m_baseDynamics.resize(6, m_v+m_k);
m_baseDynamics.resize(m_u, m_v+m_k);
for(HQPData::iterator it=m_hqpData.begin(); it!=m_hqpData.end(); it++)
{
for(ConstraintLevel::iterator itt=it->begin(); itt!=it->end(); itt++)
......@@ -347,12 +348,12 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
cl->forceRegTask->vector() = fr.vector();
}
const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-6);
const Vector & h_a = m_robot.nonLinearEffects(m_data).tail(m_v-6);
const Matrix & J_a = m_Jc.rightCols(m_v-6);
const Matrix & M_u = m_robot.mass(m_data).topRows<6>();
const Vector & h_u = m_robot.nonLinearEffects(m_data).head<6>();
const Matrix & J_u = m_Jc.leftCols<6>();
const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-m_u);
const Vector & h_a = m_robot.nonLinearEffects(m_data).tail(m_v-m_u);
const Matrix & J_a = m_Jc.rightCols(m_v-m_u);
const Matrix & M_u = m_robot.mass(m_data).topRows(m_u);
const Vector & h_u = m_robot.nonLinearEffects(m_data).head(m_u);
const Matrix & J_u = m_Jc.leftCols(m_u);
m_baseDynamics.matrix().leftCols(m_v) = M_u;
m_baseDynamics.matrix().rightCols(m_k) = -J_u.transpose();
......@@ -425,9 +426,9 @@ bool InverseDynamicsFormulationAccForce::decodeSolution(const HQPOutput & sol)
{
if(m_solutionDecoded)
return true;
const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-6);
const Vector & h_a = m_robot.nonLinearEffects(m_data).tail(m_v-6);
const Matrix & J_a = m_Jc.rightCols(m_v-6);
const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-m_u);
const Vector & h_a = m_robot.nonLinearEffects(m_data).tail(m_v-m_u);
const Matrix & J_a = m_Jc.rightCols(m_v-m_u);
m_dv = sol.x.head(m_v);
m_f = sol.x.tail(m_k);
m_tau = h_a;
......
Markdown is supported
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