Commit 96f0c8f9 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[c++] Add asserts in RobotWrapper methods to check whether joint index and...

[c++] Add asserts in RobotWrapper methods to check whether joint index and frame index are within bounds. Replace deprecated method pinocchio::framesForwardKinematics with its new version.
parent 6223fc96
......@@ -88,7 +88,7 @@ namespace tsid
// computeAllTerms does not compute the com acceleration, so we need to call centerOfMass
// Check this line, calling with zero acceleration at the last phase compute the CoM acceleration.
// pinocchio::centerOfMass(m_model, data, q,v,false);
pinocchio::framesForwardKinematics(m_model, data);
pinocchio::updateFramePlacements(m_model, data);
pinocchio::centerOfMass(m_model, data, q, v, Eigen::VectorXd::Zero(nv()));
pinocchio::ccrba(m_model, data, q, v);
}
......@@ -168,18 +168,21 @@ namespace tsid
const SE3 & RobotWrapper::position(const Data & data,
const Model::JointIndex index) const
{
assert(index>=0 && index<data.oMi.size());
return data.oMi[index];
}
const Motion & RobotWrapper::velocity(const Data & data,
const Model::JointIndex index) const
{
assert(index>=0 && index<data.v.size());
return data.v[index];
}
const Motion & RobotWrapper::acceleration(const Data & data,
const Model::JointIndex index) const
{
assert(index>=0 && index<data.a.size());
return data.a[index];
}
......@@ -187,6 +190,7 @@ namespace tsid
const Model::JointIndex index,
Data::Matrix6x & J) const
{
assert(index>=0 && index<data.oMi.size());
return pinocchio::getJointJacobian(m_model, data, index, pinocchio::WORLD, J);
}
......@@ -194,12 +198,14 @@ namespace tsid
const Model::JointIndex index,
Data::Matrix6x & J) const
{
assert(index>=0 && index<data.oMi.size());
return pinocchio::getJointJacobian(m_model, data, index, pinocchio::LOCAL, J);
}
SE3 RobotWrapper::framePosition(const Data & data,
const Model::FrameIndex index) const
{
assert(index>=0 && index<m_model.frames.size());
const Frame & f = m_model.frames[index];
return data.oMi[f.parent].act(f.placement);
}
......@@ -208,6 +214,7 @@ namespace tsid
const Model::FrameIndex index,
SE3 & framePosition) const
{
assert(index>=0 && index<m_model.frames.size());
const Frame & f = m_model.frames[index];
framePosition = data.oMi[f.parent].act(f.placement);
}
......@@ -215,6 +222,7 @@ namespace tsid
Motion RobotWrapper::frameVelocity(const Data & data,
const Model::FrameIndex index) const
{
assert(index>=0 && index<m_model.frames.size());
const Frame & f = m_model.frames[index];
return f.placement.actInv(data.v[f.parent]);
}
......@@ -223,6 +231,7 @@ namespace tsid
const Model::FrameIndex index,
Motion & frameVelocity) const
{
assert(index>=0 && index<m_model.frames.size());
const Frame & f = m_model.frames[index];
frameVelocity = f.placement.actInv(data.v[f.parent]);
}
......@@ -242,6 +251,7 @@ namespace tsid
Motion RobotWrapper::frameAcceleration(const Data & data,
const Model::FrameIndex index) const
{
assert(index>=0 && index<m_model.frames.size());
const Frame & f = m_model.frames[index];
return f.placement.actInv(data.a[f.parent]);
}
......@@ -250,6 +260,7 @@ namespace tsid
const Model::FrameIndex index,
Motion & frameAcceleration) const
{
assert(index>=0 && index<m_model.frames.size());
const Frame & f = m_model.frames[index];
frameAcceleration = f.placement.actInv(data.a[f.parent]);
}
......@@ -269,6 +280,7 @@ namespace tsid
Motion RobotWrapper::frameClassicAcceleration(const Data & data,
const Model::FrameIndex index) const
{
assert(index>=0 && index<m_model.frames.size());
const Frame & f = m_model.frames[index];
Motion a = f.placement.actInv(data.a[f.parent]);
Motion v = f.placement.actInv(data.v[f.parent]);
......@@ -280,6 +292,7 @@ namespace tsid
const Model::FrameIndex index,
Motion & frameAcceleration) const
{
assert(index>=0 && index<m_model.frames.size());
const Frame & f = m_model.frames[index];
frameAcceleration = f.placement.actInv(data.a[f.parent]);
Motion v = f.placement.actInv(data.v[f.parent]);
......@@ -302,6 +315,7 @@ namespace tsid
const Model::FrameIndex index,
Data::Matrix6x & J) const
{
assert(index>=0 && index<m_model.frames.size());
return pinocchio::getFrameJacobian(m_model, data, index, pinocchio::WORLD, J);
}
......@@ -309,6 +323,7 @@ namespace tsid
const Model::FrameIndex index,
Data::Matrix6x & J) const
{
assert(index>=0 && index<m_model.frames.size());
return pinocchio::getFrameJacobian(m_model, data, index, pinocchio::LOCAL, J);
}
......@@ -323,8 +338,8 @@ namespace tsid
void RobotWrapper::setGravity(const Motion & gravity)
{
m_model.gravity = gravity;
}
m_model.gravity = gravity;
}
// const Vector3 & com(Data & data,const Vector & q,
// const bool computeSubtreeComs = true,
......
Supports Markdown
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