Commit b705af7d authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Extend RobotWrapper to fixed-base robots: if no floating-base root joint is...

Extend RobotWrapper to fixed-base robots: if no floating-base root joint is specified, assume robot is fully actuated. Add method na to get number of actuators. Add methods to get frame vel/acc in local world-oriented frame (useful for plotting).
parent e3000ee4
......@@ -48,6 +48,7 @@ namespace tsid
.def(bp::init<std::string, std_vec, pinocchio::JointModelVariant, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("roottype"), bp::arg("verbose")), "Default constructor without RootJoint."))
.add_property("nq", &Robot::nq)
.add_property("nv", &Robot::nv)
.add_property("na", &Robot::na)
.def("model", &RobotPythonVisitor::model)
.def("data", &RobotPythonVisitor::data)
......@@ -72,6 +73,9 @@ namespace tsid
.def("frameVelocity", &RobotPythonVisitor::frameVelocity, bp::args("data", "index"))
.def("frameAcceleration", &RobotPythonVisitor::frameAcceleration, bp::args("data", "index"))
.def("frameClassicAcceleration", &RobotPythonVisitor::frameClassicAcceleration, bp::args("data", "index"))
.def("frameVelocityWorldOriented", &RobotPythonVisitor::frameVelocityWorldOriented, bp::args("data", "index"))
.def("frameAccelerationWorldOriented", &RobotPythonVisitor::frameAccelerationWorldOriented, bp::args("data", "index"))
.def("frameClassicAccelerationWorldOriented", &RobotPythonVisitor::frameClassicAccelerationWorldOriented, bp::args("data", "index"))
;
}
......@@ -137,7 +141,15 @@ namespace tsid
static pinocchio::Motion frameClassicAcceleration(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameClassicAcceleration(data, index);
}
static pinocchio::Motion frameVelocityWorldOriented(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameVelocityWorldOriented(data, index);
}
static pinocchio::Motion frameAccelerationWorldOriented(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameAccelerationWorldOriented(data, index);
}
static pinocchio::Motion frameClassicAccelerationWorldOriented(const Robot & self, const pinocchio::Data & data, const pinocchio::Model::FrameIndex & index){
return self.frameClassicAccelerationWorldOriented(data, index);
}
static void expose(const std::string & class_name)
{
std::string doc = "Robot Wrapper info.";
......
......@@ -69,6 +69,7 @@ namespace tsid
virtual int nq() const;
virtual int nv() const;
virtual int na() const;
///
/// \brief Accessor to model.
......@@ -129,6 +130,9 @@ namespace tsid
Motion frameVelocity(const Data & data,
const Model::FrameIndex index) const;
Motion frameVelocityWorldOriented(const Data & data,
const Model::FrameIndex index) const;
void frameVelocity(const Data & data,
const Model::FrameIndex index,
......@@ -137,6 +141,9 @@ namespace tsid
Motion frameAcceleration(const Data & data,
const Model::FrameIndex index) const;
Motion frameAccelerationWorldOriented(const Data & data,
const Model::FrameIndex index) const;
void frameAcceleration(const Data & data,
const Model::FrameIndex index,
Motion & frameAcceleration) const;
......@@ -144,6 +151,9 @@ namespace tsid
Motion frameClassicAcceleration(const Data & data,
const Model::FrameIndex index) const;
Motion frameClassicAccelerationWorldOriented(const Data & data,
const Model::FrameIndex index) const;
void frameClassicAcceleration(const Data & data,
const Model::FrameIndex index,
Motion & frameAcceleration) const;
......@@ -158,6 +168,7 @@ namespace tsid
protected:
void init();
void updateMd();
......@@ -166,6 +177,7 @@ namespace tsid
std::string m_model_filename;
bool m_verbose;
int m_na; /// number of actuators (nv for fixed-based, nv-6 for floating-base robots)
Vector m_rotor_inertias;
Vector m_gear_ratios;
Vector m_Md; /// diagonal part of inertia matrix due to rotor inertias
......
......@@ -39,20 +39,17 @@ namespace tsid
{
pinocchio::urdf::buildModel(filename, m_model, m_verbose);
m_model_filename = filename;
m_rotor_inertias.setZero(m_model.nv);
m_gear_ratios.setZero(m_model.nv);
m_Md.setZero(m_model.nv);
m_M.setZero(m_model.nv, m_model.nv);
m_na = m_model.nv;
init();
}
RobotWrapper::RobotWrapper(const pinocchio::Model& m, bool verbose)
: m_verbose(verbose)
{
m_model = m;
m_model_filename = "";
m_rotor_inertias.setZero(m_model.nv);
m_gear_ratios.setZero(m_model.nv);
m_Md.setZero(m_model.nv);
m_M.setZero(m_model.nv, m_model.nv);
m_na = m_model.nv-6; // for backward compatibility assume the robot has a floating base
init();
}
RobotWrapper::RobotWrapper(const std::string & filename,
......@@ -63,14 +60,21 @@ namespace tsid
{
pinocchio::urdf::buildModel(filename, rootJoint, m_model, m_verbose);
m_model_filename = filename;
m_rotor_inertias.setZero(m_model.nv-6);
m_gear_ratios.setZero(m_model.nv-6);
m_Md.setZero(m_model.nv-6);
m_na = m_model.nv-6;
init();
}
void RobotWrapper::init()
{
m_rotor_inertias.setZero(m_na);
m_gear_ratios.setZero(m_na);
m_Md.setZero(m_na);
m_M.setZero(m_model.nv, m_model.nv);
}
int RobotWrapper::nq() const { return m_model.nq; }
int RobotWrapper::nv() const { return m_model.nv; }
int RobotWrapper::na() const { return m_na; }
const Model & RobotWrapper::model() const { return m_model; }
Model & RobotWrapper::model() { return m_model; }
......@@ -150,7 +154,7 @@ namespace tsid
const Matrix & RobotWrapper::mass(const Data & data)
{
m_M = data.M;
m_M.diagonal().tail(m_model.nv-6) += m_Md;
m_M.diagonal().tail(m_na) += m_Md;
return m_M;
}
......@@ -220,6 +224,18 @@ namespace tsid
const Frame & f = m_model.frames[index];
frameVelocity = f.placement.actInv(data.v[f.parent]);
}
Motion RobotWrapper::frameVelocityWorldOriented(const Data & data,
const Model::FrameIndex index) const
{
Motion v_local, v_world;
SE3 oMi, oMi_rotation_only;
framePosition(data, index, oMi);
frameVelocity(data, index, v_local);
oMi_rotation_only.rotation(oMi.rotation());
v_world = oMi_rotation_only.act(v_local);
return v_world;
}
Motion RobotWrapper::frameAcceleration(const Data & data,
const Model::FrameIndex index) const
......@@ -235,6 +251,18 @@ namespace tsid
const Frame & f = m_model.frames[index];
frameAcceleration = f.placement.actInv(data.a[f.parent]);
}
Motion RobotWrapper::frameAccelerationWorldOriented(const Data & data,
const Model::FrameIndex index) const
{
Motion a_local, a_world;
SE3 oMi, oMi_rotation_only;
framePosition(data, index, oMi);
frameAcceleration(data, index, a_local);
oMi_rotation_only.rotation(oMi.rotation());
a_world = oMi_rotation_only.act(a_local);
return a_world;
}
Motion RobotWrapper::frameClassicAcceleration(const Data & data,
const Model::FrameIndex index) const
......@@ -255,6 +283,18 @@ namespace tsid
Motion v = f.placement.actInv(data.v[f.parent]);
frameAcceleration.linear() += v.angular().cross(v.linear());
}
Motion RobotWrapper::frameClassicAccelerationWorldOriented(const Data & data,
const Model::FrameIndex index) const
{
Motion a_local, a_world;
SE3 oMi, oMi_rotation_only;
framePosition(data, index, oMi);
frameClassicAcceleration(data, index, a_local);
oMi_rotation_only.rotation(oMi.rotation());
a_world = oMi_rotation_only.act(a_local);
return a_world;
}
void RobotWrapper::frameJacobianWorld(const Data & data,
const Model::FrameIndex index,
......
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