Commit 281bc82b authored by Julian Viereck's avatar Julian Viereck
Browse files

Reduce the constraint size of task-se3-equality based on provided mask, assume...

Reduce the constraint size of task-se3-equality based on provided mask, assume fixed mask for contact-point, rotate position error into world-oriented frame
parent 6e37a091
......@@ -111,7 +111,6 @@ namespace tsid
* local world oriented
*/
void useLocalFrame(bool local_frame);
void setMotionMask(math::ConstRefVector mask);
protected:
......
......@@ -63,6 +63,8 @@ namespace tsid
const Vector & getDesiredAcceleration() const;
Vector getAcceleration(ConstRefVector dv) const;
virtual void setMask(math::ConstRefVector mask) override;
const Vector & position_error() const;
const Vector & velocity_error() const;
const Vector & position() const;
......
......@@ -45,11 +45,15 @@ ContactPoint::ContactPoint(const std::string & name,
m_regularizationTaskWeight(regularizationTaskWeight)
{
m_weightForceRegTask << 1, 1, 1e-3;
m_forceGenMat.resize(6,3);
m_forceGenMat.resize(3, 3);
m_fRef = Vector3::Zero();
updateForceGeneratorMatrix();
updateForceInequalityConstraints();
updateForceRegularizationTask();
math::Vector motion_mask(6);
motion_mask << 1., 1., 1., 0., 0., 0.;
m_motionTask.setMask(motion_mask);
}
void ContactPoint::useLocalFrame(bool local_frame)
......@@ -57,11 +61,6 @@ void ContactPoint::useLocalFrame(bool local_frame)
m_motionTask.useLocalFrame(local_frame);
}
void ContactPoint::setMotionMask(math::ConstRefVector mask)
{
m_motionTask.setMask(mask);
}
void ContactPoint::updateForceInequalityConstraints()
{
Vector3 t1, t2;
......@@ -117,7 +116,7 @@ void ContactPoint:: updateForceGeneratorMatrix()
m_forceGenMat.setIdentity();
}
unsigned int ContactPoint::n_motion() const { return 6; }
unsigned int ContactPoint::n_motion() const { return m_motionTask.dim(); }
unsigned int ContactPoint::n_force() const { return 3; }
const Vector & ContactPoint::Kp() const { return m_motionTask.Kp(); }
......
......@@ -60,10 +60,15 @@ namespace tsid
m_local_frame = true;
}
void TaskSE3Equality::setMask(math::ConstRefVector mask)
{
TaskMotion::setMask(mask);
m_constraint.resize(dim(), m_J.cols());
}
int TaskSE3Equality::dim() const
{
//return self._mask.sum ()
return 6;
return m_mask.sum();
}
const Vector & TaskSE3Equality::Kp() const { return m_Kp; }
......@@ -177,16 +182,18 @@ namespace tsid
m_v_error = v_frame - m_wMl.actInv(m_v_ref); // vel err in local frame
// desired acc in local frame
m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector())
m_a_des = - m_Kp.cwiseProduct(m_p_error_vec)
- m_Kd.cwiseProduct(m_v_error.toVector())
+ m_wMl.actInv(m_a_ref).toVector();
} else {
m_p_error_vec = m_wMl.toActionMatrix() * // pos err in local world-oriented frame
m_p_error.toVector();
m_v_error = m_wMl.act(v_frame) - m_v_ref; // vel err in local world-oriented frame
m_drift = m_wMl.act(m_drift);
// desired acc in local oriented frame
m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector())
m_a_des = - m_Kp.cwiseProduct(m_p_error_vec)
- m_Kd.cwiseProduct(m_v_error.toVector())
+ m_a_ref.toVector();
......@@ -199,8 +206,18 @@ namespace tsid
m_v_ref_vec = m_v_ref.toVector();
m_v = v_frame.toVector();
m_constraint.setMatrix(m_mask.asDiagonal() * m_J);
m_constraint.setVector(m_mask.asDiagonal() * (m_a_des - m_drift.toVector()));
m_a_des -= m_drift.toVector();
int idx = 0;
for (int i = 0; i < 6; i++) {
if (m_mask(i) != 1.) continue;
m_constraint.matrix().row(idx) = m_J.row(idx);
m_constraint.vector().row(idx) = m_a_des.row(idx);
idx += 1;
}
return m_constraint;
}
}
......
......@@ -162,7 +162,6 @@ class StandardRomeoInvDynCtrl
postureTask->Kd(2.0*postureTask->Kp().cwiseSqrt());
tsid->addMotionTask(*postureTask, w_posture, 1);
}
};
const double StandardRomeoInvDynCtrl::lxp = 0.14;
......@@ -553,16 +552,12 @@ BOOST_AUTO_TEST_CASE ( test_contact_point_invdyn_formulation_acc_force )
std::array<ContactPoint*, 4> contacts;
for (int i = 0; i < 4; i++) {
math::Vector motion_mask(6);
motion_mask << 1., 1., 1., 0., 0., 0.;
ContactPoint* cp = new ContactPoint("contact_" + contactFrames[i], robot,
contactFrames[i], contactNormal, mu, fMin, fMax, w_forceRef);
cp->Kp(kp_contact*Vector::Ones(6));
cp->Kd(2.0*cp->Kp().cwiseSqrt());
cp->setReference(robot.framePosition(data, robot.model().getFrameId(contactFrames[i])));
cp->useLocalFrame(false);
cp->setMotionMask(motion_mask);
tsid->addRigidContact(*cp, 1);
contacts[i] = cp;
......
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