Commit 9a58a10a authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[frames] renaming, bindings, RobotWrapper, deprecating

parent f0ef7326
...@@ -45,7 +45,7 @@ namespace se3 ...@@ -45,7 +45,7 @@ namespace se3
) )
{ {
computeJointJacobians(model,data,q); computeJointJacobians(model,data,q);
framesForwardKinematics(model,data); updateFramePlacements(model,data);
return get_frame_jacobian_proxy(model, data, frame_id, rf); return get_frame_jacobian_proxy(model, data, frame_id, rf);
} }
...@@ -74,21 +74,59 @@ namespace se3 ...@@ -74,21 +74,59 @@ namespace se3
) )
{ {
computeJointJacobiansTimeVariation(model,data,q,v); computeJointJacobiansTimeVariation(model,data,q,v);
framesForwardKinematics(model,data); updateFramePlacements(model,data);
return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf); 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() void exposeFramesAlgo()
{ {
bp::def("framesKinematics", bp::def("updateFramePlacements",
(void (*)(const Model &, Data &))&framesForwardKinematics, (void (*)(const Model &, Data &))&updateFramePlacements,
bp::args("Model","Data"), bp::args("Model","Data"),
"Computes the placements of all the operational frames according to the current joint placement stored in 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, (void (*)(const Model &, Data &, const Eigen::VectorXd &))&framesForwardKinematics,
bp::args("Model","Data", bp::args("Model","Data",
"Configuration q (size Model::nq)"), "Configuration q (size Model::nq)"),
......
...@@ -21,6 +21,13 @@ from __future__ import print_function ...@@ -21,6 +21,13 @@ from __future__ import print_function
from . import libpinocchio_pywrap as se3 from . import libpinocchio_pywrap as se3
from .deprecation import deprecated 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.") @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): def computeJacobians(model,data,q=None):
if q is None: if q is None:
......
...@@ -99,7 +99,11 @@ class RobotWrapper(object): ...@@ -99,7 +99,11 @@ class RobotWrapper(object):
else: else:
se3.forwardKinematics(self.model, self.data, q) 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): 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: if update_kinematics:
se3.forwardKinematics(self.model, self.data, q) se3.forwardKinematics(self.model, self.data, q)
return self.data.oMi[index] return self.data.oMi[index]
...@@ -114,32 +118,29 @@ class RobotWrapper(object): ...@@ -114,32 +118,29 @@ class RobotWrapper(object):
se3.forwardKinematics(self.model, self.data, q, v, a) se3.forwardKinematics(self.model, self.data, q, v, a)
return self.data.a[index] return self.data.a[index]
@deprecated("This method is now renamed framePlacement. Please use framePlacement instead.")
def framePosition(self, q, index, update_kinematics=True): 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: if update_kinematics:
se3.forwardKinematics(self.model, self.data, q) se3.forwardKinematics(self.model, self.data, q)
frame = self.model.frames[index] return se3.updateFramePlacement(self.model, self.data, index)
parentPos = self.data.oMi[frame.parent]
return parentPos.act(frame.placement)
def frameVelocity(self, q, v, index, update_kinematics=True): def frameVelocity(self, q, v, index, update_kinematics=True):
if update_kinematics: if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v) se3.forwardKinematics(self.model, self.data, q, v)
frame = self.model.frames[index] return se3.getFrameVelocity(self.model, self.data, index)
parentJointVel = self.data.v[frame.parent]
return frame.placement.actInv(parentJointVel)
def frameAcceleration(self, q, v, a, index, update_kinematics=True): def frameAcceleration(self, q, v, a, index, update_kinematics=True):
if update_kinematics: if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v, a) se3.forwardKinematics(self.model, self.data, q, v, a)
frame = self.model.frames[index] return se3.getFrameAcceleration(self.model, self.data, index)
parentJointAcc = self.data.a[frame.parent]
return frame.placement.actInv(parentJointAcc)
def frameClassicAcceleration(self, index): def frameClassicAcceleration(self, index):
f = self.model.frames[index] v = se3.getFrameVelocity(self.model, self.data, index)
a = f.placement.actInv(self.data.a[f.parent]) a = se3.getFrameAcceleration(self.model, self.data, index)
v = f.placement.actInv(self.data.v[f.parent]) a.linear += np.cross(v.angular, v.linear, axis=0)
a.linear += np.cross(v.angular.T, v.linear.T).T
return a; return a;
@deprecated("This method is now deprecated. Please use jointJacobian instead. It will be removed in release 1.4.0 of Pinocchio.") @deprecated("This method is now deprecated. Please use jointJacobian instead. It will be removed in release 1.4.0 of Pinocchio.")
...@@ -175,9 +176,12 @@ class RobotWrapper(object): ...@@ -175,9 +176,12 @@ class RobotWrapper(object):
else: else:
se3.updateGeometryPlacements(self.model, self.data, geom_model, geom_data) 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): 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 It computes the Jacobian of frame given by its id (frame_id) either expressed in the
...@@ -188,7 +192,7 @@ class RobotWrapper(object): ...@@ -188,7 +192,7 @@ class RobotWrapper(object):
''' '''
Similar to getFrameJacobian but it also calls before se3.computeJointJacobians and 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): def frameJacobian(self, q, frame_id, rf_frame):
return se3.frameJacobian(self.model, self.data, q, frame_id, rf_frame) return se3.frameJacobian(self.model, self.data, q, frame_id, rf_frame)
......
...@@ -32,6 +32,35 @@ namespace se3 ...@@ -32,6 +32,35 @@ namespace se3
* *
* @warning One of the algorithms forwardKinematics should have been called first * @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, inline void framesForwardKinematics(const Model & model,
Data & data); Data & data);
...@@ -47,21 +76,6 @@ namespace se3 ...@@ -47,21 +76,6 @@ namespace se3
Data & data, Data & data,
const Eigen::VectorXd & q); const Eigen::VectorXd & q);
/**
* @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 & frameForwardKinematics(const Model & model,
Data & data,
const Model::FrameIndex frame_id);
/** /**
* @brief Returns the spatial velocity of the frame expressed in the LOCAL frame 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. * You must first call se3::forwardKinematics to update placement and velocity values in data structure.
......
...@@ -26,13 +26,13 @@ namespace se3 ...@@ -26,13 +26,13 @@ namespace se3
{ {
inline void framesForwardKinematics(const Model & model, inline void updateFramePlacements(const Model & model,
Data & data) Data & data)
{ {
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
// The following for loop starts by index 1 because the first frame is fixed // 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) for (Model::FrameIndex i=1; i < (Model::FrameIndex) model.nframes; ++i)
{ {
const Frame & frame = model.frames[i]; const Frame & frame = model.frames[i];
...@@ -43,20 +43,10 @@ namespace se3 ...@@ -43,20 +43,10 @@ namespace se3
data.oMf[i] = data.oMi[parent]*frame.placement; data.oMf[i] = data.oMi[parent]*frame.placement;
} }
} }
inline void framesForwardKinematics(const Model & model,
Data & data,
const Eigen::VectorXd & q)
{
assert(model.check(data) && "data is not consistent with model.");
forwardKinematics(model, data, q);
framesForwardKinematics(model, data);
}
inline const SE3 & frameForwardKinematics(const Model & model, inline const SE3 & updateFramePlacement(const Model & model,
Data & data, Data & data,
const Model::FrameIndex frame_id) const Model::FrameIndex frame_id)
{ {
const Frame & frame = model.frames[frame_id]; const Frame & frame = model.frames[frame_id];
const Model::JointIndex & parent = frame.parent; const Model::JointIndex & parent = frame.parent;
...@@ -67,6 +57,22 @@ namespace se3 ...@@ -67,6 +57,22 @@ namespace se3
return data.oMf[frame_id]; 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)
{
assert(model.check(data) && "data is not consistent with model.");
forwardKinematics(model, data, q);
updateFramePlacements(model, data);
}
void getFrameVelocity(const Model & model, void getFrameVelocity(const Model & model,
const Data & data, const Data & data,
const Model::FrameIndex frame_id, const Model::FrameIndex frame_id,
......
...@@ -60,7 +60,7 @@ BOOST_AUTO_TEST_CASE ( test_kinematics ) ...@@ -60,7 +60,7 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
} }
BOOST_AUTO_TEST_CASE ( test_single_kinematics ) BOOST_AUTO_TEST_CASE ( test_update_placements )
{ {
using namespace Eigen; using namespace Eigen;
using namespace se3; using namespace se3;
...@@ -79,7 +79,33 @@ BOOST_AUTO_TEST_CASE ( test_single_kinematics ) ...@@ -79,7 +79,33 @@ BOOST_AUTO_TEST_CASE ( test_single_kinematics )
q.middleRows<4> (3).normalize(); q.middleRows<4> (3).normalize();
forwardKinematics(model, data, q); forwardKinematics(model, data, q);
frameForwardKinematics(model, data, frame_idx); 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); framesForwardKinematics(model, data_ref, q);
...@@ -199,9 +225,10 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation ) ...@@ -199,9 +225,10 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
VectorXd a = VectorXd::Random(model.nv); VectorXd a = VectorXd::Random(model.nv);
computeJointJacobiansTimeVariation(model,data,q,v); computeJointJacobiansTimeVariation(model,data,q,v);
updateFramePlacements(model,data);
forwardKinematics(model,data_ref,q,v,a); forwardKinematics(model,data_ref,q,v,a);
framesForwardKinematics(model,data_ref); updateFramePlacements(model,data_ref);
framesForwardKinematics(model,data);
BOOST_CHECK(isFinite(data.dJ)); BOOST_CHECK(isFinite(data.dJ));
...@@ -250,7 +277,7 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation ) ...@@ -250,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); Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv);
J_ref_world.fill(0.); J_ref_local.fill(0.); J_ref_world.fill(0.); J_ref_local.fill(0.);
computeJointJacobians(model,data_ref,q); computeJointJacobians(model,data_ref,q);
framesForwardKinematics(model,data_ref); updateFramePlacements(model,data_ref);
const SE3 & oMf_q = data_ref.oMf[idx]; const SE3 & oMf_q = data_ref.oMf[idx];
getFrameJacobian<WORLD>(model,data_ref,idx,J_ref_world); getFrameJacobian<WORLD>(model,data_ref,idx,J_ref_world);
getFrameJacobian<LOCAL>(model,data_ref,idx,J_ref_local); getFrameJacobian<LOCAL>(model,data_ref,idx,J_ref_local);
...@@ -259,7 +286,7 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation ) ...@@ -259,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); 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.); J_ref_plus_world.fill(0.); J_ref_plus_local.fill(0.);
computeJointJacobians(model,data_ref_plus,q_plus); 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]; const SE3 & oMf_qplus = data_ref_plus.oMf[idx];
getFrameJacobian<WORLD>(model,data_ref_plus,idx,J_ref_plus_world); getFrameJacobian<WORLD>(model,data_ref_plus,idx,J_ref_plus_world);
getFrameJacobian<LOCAL>(model,data_ref_plus,idx,J_ref_plus_local); getFrameJacobian<LOCAL>(model,data_ref_plus,idx,J_ref_plus_local);
...@@ -275,7 +302,7 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation ) ...@@ -275,7 +302,7 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
//data //data
computeJointJacobiansTimeVariation(model,data,q,v); computeJointJacobiansTimeVariation(model,data,q,v);
forwardKinematics(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); Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv);
dJ_world.fill(0.); dJ_local.fill(0.); dJ_world.fill(0.); dJ_local.fill(0.);
getFrameJacobianTimeVariation<WORLD>(model,data,idx,dJ_world); 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