Unverified Commit 2e47bdb7 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #516 from gabrielebndn/devel

[frames] Helper functions for frames
parents 2ef283ef a802cdcb
......@@ -45,7 +45,7 @@ namespace se3
)
{
computeJointJacobians(model,data,q);
framesForwardKinematics(model,data);
updateFramePlacements(model,data);
return get_frame_jacobian_proxy(model, data, frame_id, rf);
}
......@@ -74,21 +74,59 @@ namespace se3
)
{
computeJointJacobiansTimeVariation(model,data,q,v);
framesForwardKinematics(model,data);
updateFramePlacements(model,data);
return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
}
static Motion get_frame_velocity_proxy(const Model & model,
Data & data,
const Model::FrameIndex frame_id
)
{
Motion v;
getFrameVelocity(model,data,frame_id,v);
return v;
}
static Motion get_frame_acceleration_proxy(const Model & model,
Data & data,
const Model::FrameIndex frame_id
)
{
Motion a;
getFrameAcceleration(model,data,frame_id,a);
return a;
}
void exposeFramesAlgo()
{
bp::def("framesKinematics",
(void (*)(const Model &, Data &))&framesForwardKinematics,
bp::def("updateFramePlacements",
(void (*)(const Model &, Data &))&updateFramePlacements,
bp::args("Model","Data"),
"Computes the placements of all the operational frames according to the current joint placement stored in data"
"and put the results in data.");
"and puts the results in data.");
bp::def("updateFramePlacement",
(const SE3 & (*)(const Model &, Data &, const Model::FrameIndex))&updateFramePlacement,
bp::args("Model","Data","Operational frame ID (int)"),
"Computes the placement of the given operational frames according to the current joint placement stored in data,"
"puts the results in data and returns it.",
bp::return_value_policy<bp::return_by_value>());
bp::def("getFrameVelocity",
(Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_velocity_proxy,
bp::args("Model","Data","Operational frame ID (int)"),
"Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
"Fist or second order forwardKinematics should be called first.");
bp::def("getFrameAcceleration",
(Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_acceleration_proxy,
bp::args("Model","Data","Operational frame ID (int)"),
"Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
"Second order forwardKinematics should be called first.");
bp::def("framesKinematics",
bp::def("framesForwardKinematics",
(void (*)(const Model &, Data &, const Eigen::VectorXd &))&framesForwardKinematics,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
......
......@@ -21,6 +21,13 @@ from __future__ import print_function
from . import libpinocchio_pywrap as se3
from .deprecation import deprecated
@deprecated("This function has been renamed updateFramePlacements when taking two arguments, and framesForwardKinematics when taking three. Please change to the appropriate method.")
def framesKinematics(model,data,q=None):
if q is None:
se3.updateFramePlacements(model,data)
else:
se3.framesForwardKinematics(model,data,q)
@deprecated("This function has been renamed computeJointJacobians and will be removed in release 1.4.0 of Pinocchio. Please change for new computeJacobians.")
def computeJacobians(model,data,q=None):
if q is None:
......
......@@ -30,6 +30,8 @@ def deprecated(instructions):
return func(*args, **kwargs)
if wrapper.__doc__ is None:
wrapper.__doc__ = instructions
return wrapper
return decorator
......@@ -115,7 +115,11 @@ class RobotWrapper(object):
else:
se3.forwardKinematics(self.model, self.data, q)
@deprecated("This method is now renamed placement. Please use placement instead.")
def position(self, q, index, update_kinematics=True):
return self.placement(q, index, update_kinematics)
def placement(self, q, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q)
return self.data.oMi[index]
......@@ -130,32 +134,29 @@ class RobotWrapper(object):
se3.forwardKinematics(self.model, self.data, q, v, a)
return self.data.a[index]
@deprecated("This method is now renamed framePlacement. Please use framePlacement instead.")
def framePosition(self, q, index, update_kinematics=True):
return self.framePlacement(q, index, update_kinematics)
def framePlacement(self, q, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q)
frame = self.model.frames[index]
parentPos = self.data.oMi[frame.parent]
return parentPos.act(frame.placement)
return se3.updateFramePlacement(self.model, self.data, index)
def frameVelocity(self, q, v, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v)
frame = self.model.frames[index]
parentJointVel = self.data.v[frame.parent]
return frame.placement.actInv(parentJointVel)
return se3.getFrameVelocity(self.model, self.data, index)
def frameAcceleration(self, q, v, a, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v, a)
frame = self.model.frames[index]
parentJointAcc = self.data.a[frame.parent]
return frame.placement.actInv(parentJointAcc)
return se3.getFrameAcceleration(self.model, self.data, index)
def frameClassicAcceleration(self, index):
f = self.model.frames[index]
a = f.placement.actInv(self.data.a[f.parent])
v = f.placement.actInv(self.data.v[f.parent])
a.linear += np.cross(v.angular.T, v.linear.T).T
v = se3.getFrameVelocity(self.model, self.data, index)
a = se3.getFrameAcceleration(self.model, self.data, index)
a.linear += np.cross(v.angular, v.linear, axis=0)
return a;
@deprecated("This method is now deprecated. Please use jointJacobian instead. It will be removed in release 1.4.0 of Pinocchio.")
......@@ -191,9 +192,12 @@ class RobotWrapper(object):
else:
se3.updateGeometryPlacements(self.model, self.data, geom_model, geom_data)
@deprecated("This method is now renamed framesForwardKinematics. Please use framesForwardKinematics instead.")
def framesKinematics(self, q):
se3.framesKinematics(self.model, self.data, q)
se3.framesForwardKinematics(self.model, self.data, q)
def framesForwardKinematics(self, q):
se3.framesForwardKinematics(self.model, self.data, q)
'''
It computes the Jacobian of frame given by its id (frame_id) either expressed in the
......@@ -204,7 +208,7 @@ class RobotWrapper(object):
'''
Similar to getFrameJacobian but it also calls before se3.computeJointJacobians and
se3.framesKinematics to update internal value of self.data related to frames.
se3.framesForwardKinematics to update internal value of self.data related to frames.
'''
def frameJacobian(self, q, frame_id, rf_frame):
return se3.frameJacobian(self.model, self.data, q, frame_id, rf_frame)
......
......@@ -25,13 +25,42 @@ namespace se3
{
/**
* @brief Updates the position of each frame contained in the model
* @brief Updates the placement of each frame contained in the model
*
* @param[in] model The kinematic model.
* @param data Data associated to model.
*
* @warning One of the algorithms forwardKinematics should have been called first
*/
inline void updateFramePlacements(const Model & model,
Data & data);
/**
* @brief Updates the placement of the given frame.
*
* @param[in] model The kinematic model.
* @param data Data associated to model.
* @param[in] frame_id Id of the operational Frame.
*
* @return A reference to the frame placement stored in data.oMf[frame_id]
*
* @warning One of the algorithms forwardKinematics should have been called first
*/
inline const SE3 & updateFramePlacement(const Model & model,
Data & data,
const Model::FrameIndex frame_id);
/**
* @brief Updates the placement of each frame contained in the model
*
* @deprecated This function is now deprecated. Please call se3::updateFramePlacements for same functionality
*
* @param[in] model The kinematic model.
* @param data Data associated to model.
*
* @warning One of the algorithms forwardKinematics should have been called first
*/
PINOCCHIO_DEPRECATED
inline void framesForwardKinematics(const Model & model,
Data & data);
......@@ -48,14 +77,47 @@ namespace se3
const Eigen::VectorXd & q);
/**
* @brief Returns the jacobian of the frame expresssed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
* @brief Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system.
* You must first call se3::forwardKinematics to update placement and velocity values in data structure.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational Frame
* @param[out] frame_v The spatial velocity of the Frame expressed in the coordinates Frame.
*
* @warning Fist or second order forwardKinematics should have been called first
*/
void getFrameVelocity(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Motion & frame_v);
/**
* @brief Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
* You must first call se3::forwardKinematics to update placement values in data structure.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational Frame
* @param[out] frame_a The spatial acceleration of the Frame expressed in the coordinates Frame.
*
* @warning Second order forwardKinematics should have been called first
*/
void getFrameAcceleration(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Motion & frame_a);
/**
* @brief Returns the jacobian of the frame expressed either in the LOCAL frame coordinate system or in the WORLD coordinate system,
* depending on the value of the template parameter rf.
* You must first call se3::computeJointJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
*
* @tparam rf Reference frame in which the columns of the Jacobian are expressed.
*
* @remark Similarly to se3::getJointJacobian with LOCAL or WORLD templated parameters, if rf == LOCAL, this function returns the Jacobian of the frame expressed
* in the local coordinates of the frame, or if rl == WORDL, it returns the Jacobian expressed of the point coincident with the origin
* in the local coordinates of the frame, or if rl == WORLD, it returns the Jacobian expressed of the point coincident with the origin
* and expressed in a coordinate system aligned with the WORLD.
*
* @param[in] model The kinematic model
......@@ -63,7 +125,7 @@ namespace se3
* @param[in] frame_id Id of the operational Frame
* @param[out] J The Jacobian of the Frame expressed in the coordinates Frame.
*
* @warning The function se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
* @warning The functions se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
*/
template<ReferenceFrame rf>
void getFrameJacobian(const Model & model,
......
......@@ -26,13 +26,13 @@ namespace se3
{
inline void framesForwardKinematics(const Model & model,
Data & data)
inline void updateFramePlacements(const Model & model,
Data & data)
{
assert(model.check(data) && "data is not consistent with model.");
// The following for loop starts by index 1 because the first frame is fixed
// and corresponds to the universe.s
// and corresponds to the universe
for (Model::FrameIndex i=1; i < (Model::FrameIndex) model.nframes; ++i)
{
const Frame & frame = model.frames[i];
......@@ -43,7 +43,26 @@ namespace se3
data.oMf[i] = data.oMi[parent]*frame.placement;
}
}
inline const SE3 & updateFramePlacement(const Model & model,
Data & data,
const Model::FrameIndex frame_id)
{
const Frame & frame = model.frames[frame_id];
const Model::JointIndex & parent = frame.parent;
if (frame.placement.isIdentity())
data.oMf[frame_id] = data.oMi[parent];
else
data.oMf[frame_id] = data.oMi[parent]*frame.placement;
return data.oMf[frame_id];
}
inline void framesForwardKinematics(const Model & model,
Data & data)
{
updateFramePlacements(model,data);
}
inline void framesForwardKinematics(const Model & model,
Data & data,
const Eigen::VectorXd & q)
......@@ -51,7 +70,31 @@ namespace se3
assert(model.check(data) && "data is not consistent with model.");
forwardKinematics(model, data, q);
framesForwardKinematics(model, data);
updateFramePlacements(model, data);
}
void getFrameVelocity(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Motion & frame_v)
{
assert(model.check(data) && "data is not consistent with model.");
const Frame & frame = model.frames[frame_id];
const Model::JointIndex & parent = frame.parent;
frame_v = frame.placement.actInv(data.v[parent]);
}
void getFrameAcceleration(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Motion & frame_a)
{
assert(model.check(data) && "data is not consistent with model.");
const Frame & frame = model.frames[frame_id];
const Model::JointIndex & parent = frame.parent;
frame_a = frame.placement.actInv(data.a[parent]);
}
template<ReferenceFrame rf>
......
......@@ -200,7 +200,7 @@ namespace se3
const Model::JointIndex jointId)
{
data.J.setZero();
jacobian(model,data,q,jointId,data.J);
jointJacobian(model,data,q,jointId,data.J);
return data.J;
}
......
......@@ -60,6 +60,108 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
}
BOOST_AUTO_TEST_CASE ( test_update_placements )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
se3::Data data(model);
se3::Data data_ref(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
forwardKinematics(model, data, q);
updateFramePlacements(model, data);
framesForwardKinematics(model, data_ref, q);
BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
}
BOOST_AUTO_TEST_CASE ( test_update_single_placement )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
se3::Data data(model);
se3::Data data_ref(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
forwardKinematics(model, data, q);
updateFramePlacement(model, data, frame_idx);
framesForwardKinematics(model, data_ref, q);
BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
}
BOOST_AUTO_TEST_CASE ( test_velocity )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
se3::Data data(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd v = VectorXd::Ones(model.nv);
forwardKinematics(model, data, q, v);
Motion vf;
getFrameVelocity(model, data, frame_idx, vf);
BOOST_CHECK(vf.isApprox(framePlacement.actInv(data.v[parent_idx])));
}
BOOST_AUTO_TEST_CASE ( test_acceleration )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
Model::FrameIndex frame_idx = model.getFrameId(frame_name);
se3::Data data(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd v = VectorXd::Ones(model.nv);
VectorXd a = VectorXd::Ones(model.nv);
forwardKinematics(model, data, q, v, a);
Motion af;
getFrameAcceleration(model, data, frame_idx, af);
BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx])));
}
BOOST_AUTO_TEST_CASE ( test_jacobian )
{
......@@ -86,7 +188,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
getFrameJacobian(model,data,idx,Jff);
getFrameJacobian<LOCAL>(model,data,idx,Jff);
getJointJacobian<LOCAL>(model, data_ref, parent_idx, Jjj);
Motion nu_frame = Motion(Jff*q_dot);
......@@ -123,9 +225,10 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
VectorXd a = VectorXd::Random(model.nv);
computeJointJacobiansTimeVariation(model,data,q,v);
updateFramePlacements(model,data);
forwardKinematics(model,data_ref,q,v,a);
framesForwardKinematics(model,data_ref);
framesForwardKinematics(model,data);
updateFramePlacements(model,data_ref);
BOOST_CHECK(isFinite(data.dJ));
......@@ -174,7 +277,7 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv);
J_ref_world.fill(0.); J_ref_local.fill(0.);
computeJointJacobians(model,data_ref,q);
framesForwardKinematics(model,data_ref);
updateFramePlacements(model,data_ref);
const SE3 & oMf_q = data_ref.oMf[idx];
getFrameJacobian<WORLD>(model,data_ref,idx,J_ref_world);
getFrameJacobian<LOCAL>(model,data_ref,idx,J_ref_local);
......@@ -183,7 +286,7 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
Data::Matrix6x J_ref_plus_world(6,model.nv), J_ref_plus_local(6,model.nv);
J_ref_plus_world.fill(0.); J_ref_plus_local.fill(0.);
computeJointJacobians(model,data_ref_plus,q_plus);
framesForwardKinematics(model,data_ref_plus);
updateFramePlacements(model,data_ref_plus);
const SE3 & oMf_qplus = data_ref_plus.oMf[idx];
getFrameJacobian<WORLD>(model,data_ref_plus,idx,J_ref_plus_world);
getFrameJacobian<LOCAL>(model,data_ref_plus,idx,J_ref_plus_local);
......@@ -199,7 +302,7 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
//data
computeJointJacobiansTimeVariation(model,data,q,v);
forwardKinematics(model,data,q,v);
framesForwardKinematics(model,data);
updateFramePlacements(model,data);
Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv);
dJ_world.fill(0.); dJ_local.fill(0.);
getFrameJacobianTimeVariation<WORLD>(model,data,idx,dJ_world);
......
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