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
)
{
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:
......
......@@ -99,7 +99,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]
......@@ -114,32 +118,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.")
......@@ -175,9 +176,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
......@@ -188,7 +192,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)
......
......@@ -32,6 +32,35 @@ namespace se3
*
* @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);
......@@ -47,21 +76,6 @@ namespace se3
Data & data,
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.
* You must first call se3::forwardKinematics to update placement and velocity values in data structure.
......
......@@ -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,20 +43,10 @@ namespace se3
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,
Data & data,
const Model::FrameIndex frame_id)
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;
......@@ -67,6 +57,22 @@ namespace se3
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,
const Data & data,
const Model::FrameIndex frame_id,
......
......@@ -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 se3;
......@@ -79,7 +79,33 @@ BOOST_AUTO_TEST_CASE ( test_single_kinematics )
q.middleRows<4> (3).normalize();
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);
......@@ -199,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));
......@@ -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);
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);
......@@ -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);
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);
......@@ -275,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