Commit d4693af3 authored by Julian Viereck's avatar Julian Viereck
Browse files

task-se3-equaltiy: Add flag to toggle between local and local-world-oriented frame

parent 67ac0a31
......@@ -153,10 +153,6 @@ namespace tsid
const Model::FrameIndex index,
Data::Matrix6x & J) const;
void frameJacobianLocalWorldOriented(const Data & data,
const Model::FrameIndex index,
Data::Matrix6x & J) const;
protected:
void updateMd();
......
......@@ -77,6 +77,8 @@ namespace tsid
Index frame_id() const;
void useLocalFrame(bool local_frame);
protected:
std::string m_frame_name;
......@@ -94,6 +96,7 @@ namespace tsid
Matrix6x m_J;
ConstraintEquality m_constraint;
TrajectorySample m_ref;
bool m_local_frame;
};
}
......
......@@ -258,20 +258,6 @@ namespace tsid
{
return se3::getFrameJacobian(m_model, data, index, J);
}
void RobotWrapper::frameJacobianLocalWorldOriented(const Data & data,
const Model::FrameIndex index,
Data::Matrix6x & J) const
{
se3::getFrameJacobian(m_model, data, index, J);
Eigen::Matrix3d rotation = data.oMf[index].rotation();
Eigen::Vector3d translation = Eigen::Vector3d::Zero();
J = (SE3(rotation, translation).toActionMatrix() * J);
}
// const Vector3 & com(Data & data,const Vector & q,
// const bool computeSubtreeComs = true,
......
......@@ -55,6 +55,8 @@ namespace tsid
m_mask.resize(6);
m_mask.setIdentity();
m_local_frame = true;
}
int TaskSE3Equality::dim() const
......@@ -142,6 +144,11 @@ namespace tsid
return m_constraint;
}
void TaskSE3Equality::useLocalFrame(bool local_frame)
{
m_local_frame = local_frame;
}
const ConstraintBase & TaskSE3Equality::compute(const double ,
ConstRefVector ,
ConstRefVector ,
......@@ -153,37 +160,45 @@ namespace tsid
m_robot.frameVelocity(data, m_frame_id, v_frame);
m_robot.frameClassicAcceleration(data, m_frame_id, m_drift);
// Transformation from local to world
m_wMl.rotation(oMi.rotation());
// @todo Since Jacobian computation is cheaper in world frame
// we could do all computations in world frame
m_robot.frameJacobianLocal(data, m_frame_id, m_J);
errorInSE3(oMi, m_M_ref, m_p_error); // pos err in local frame
m_v_error = v_frame - m_wMl.actInv(m_v_ref); // vel err in local frame
m_p_error_vec = m_p_error.toVector();
m_v_error_vec = m_v_error.toVector();
SE3ToVector(m_M_ref, m_p_ref);
m_v_ref_vec = m_v_ref.toVector();
SE3ToVector(oMi, m_p);
m_v = v_frame.toVector();
#ifndef NDEBUG
// PRINT_VECTOR(v_frame.toVector());
// PRINT_VECTOR(m_v_ref.toVector());
#endif
// Transformation from local to world
m_wMl.rotation(oMi.rotation());
// desired acc in local frame
m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector())
- m_Kd.cwiseProduct(m_v_error.toVector())
+ m_wMl.actInv(m_a_ref).toVector();
if (m_local_frame) {
m_v_error = v_frame - m_wMl.actInv(m_v_ref); // vel err in local frame
// @todo Since Jacobian computation is cheaper in world frame
// we could do all computations in world frame
m_robot.frameJacobianLocal(data, m_frame_id, m_J);
// desired acc in local frame
m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector())
- m_Kd.cwiseProduct(m_v_error.toVector())
+ m_wMl.actInv(m_a_ref).toVector();
} else {
m_v_error = m_wMl.act(v_frame) - m_v_ref; // vel err in local 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_Kd.cwiseProduct(m_v_error.toVector())
+ m_a_ref.toVector();
m_J = m_wMl.toActionMatrix() * m_J;
}
m_v_error_vec = m_v_error.toVector();
m_v_ref_vec = m_v_ref.toVector();
m_v = v_frame.toVector();
m_constraint.setMatrix(m_mask.transpose() * m_J);
m_constraint.setVector(m_mask.transpose() * (m_a_des - m_drift.toVector()));
return m_constraint;
}
}
}
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