From 268f32edf68ab291c281a70783ca41e3df9dee04 Mon Sep 17 00:00:00 2001
From: Gabriele Buondonno <gbuondon@laas.fr>
Date: Fri, 22 Feb 2019 01:57:57 +0100
Subject: [PATCH] [frames] frameVelocity and frameAcceleration return value in
C++
---
bindings/python/algorithm/expose-frames.cpp | 24 +-----
src/algorithm/frames.hpp | 90 ++++++++++++++++-----
src/algorithm/frames.hxx | 26 +++---
unittest/frames.cpp | 6 +-
4 files changed, 85 insertions(+), 61 deletions(-)
diff --git a/bindings/python/algorithm/expose-frames.cpp b/bindings/python/algorithm/expose-frames.cpp
index 9ca24894c..3b2a793ae 100644
--- a/bindings/python/algorithm/expose-frames.cpp
+++ b/bindings/python/algorithm/expose-frames.cpp
@@ -58,26 +58,6 @@ namespace pinocchio
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()
@@ -97,13 +77,13 @@ namespace pinocchio
bp::return_value_policy<bp::return_by_value>());
bp::def("getFrameVelocity",
- (Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_velocity_proxy,
+ &getFrameVelocity<double,0,JointCollectionDefaultTpl>,
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,
+ &getFrameAcceleration<double,0,JointCollectionDefaultTpl>,
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.");
diff --git a/src/algorithm/frames.hpp b/src/algorithm/frames.hpp
index 7ed39c263..fb7fe2d81 100644
--- a/src/algorithm/frames.hpp
+++ b/src/algorithm/frames.hpp
@@ -60,6 +60,24 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q);
+ /**
+ * @brief Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system.
+ * You must first call pinocchio::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
+ *
+ * @return The spatial velocity of the Frame expressed in the coordinates Frame.
+ *
+ * @warning Fist or second order forwardKinematics should have been called first
+ */
+ template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+ inline MotionTpl<Scalar, Options>
+ getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+ const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+ const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id);
+
/**
* @brief Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system.
* You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure.
@@ -69,13 +87,58 @@ namespace pinocchio
* @param[in] frame_id Id of the operational Frame
* @param[out] frame_v The spatial velocity of the Frame expressed in the coordinates Frame.
*
+ * @deprecated This function is now deprecated. Use the return-value version instead (since: 19 feb 2019)
+ *
* @warning Fist or second order forwardKinematics should have been called first
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MotionLike>
- void getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- const DataTpl<Scalar,Options,JointCollectionTpl> & data,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
- const MotionDense<MotionLike> & frame_v);
+ PINOCCHIO_DEPRECATED
+ inline void getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+ const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+ const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
+ const MotionDense<MotionLike> & frame_v)
+ {
+ frame_v.derived() = getFrameVelocity(model, data, frame_id);
+ }
+
+ /**
+ * @brief Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
+ * You must first call pinocchio::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
+ *
+ * @return The spatial acceleration of the Frame expressed in the coordinates Frame.
+ *
+ * @warning Second order forwardKinematics should have been called first
+ */
+ template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+ inline MotionTpl<Scalar, Options>
+ getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+ const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+ const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id);
+
+ /**
+ * @brief Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
+ * You must first call pinocchio::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.
+ *
+ * @deprecated This function is now deprecated. Use the return-value version instead (since: 19 feb 2019)
+ *
+ * @warning Second order forwardKinematics should have been called first
+ */
+ template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MotionLike>
+ PINOCCHIO_DEPRECATED
+ inline void getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+ const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+ const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
+ const MotionDense<MotionLike> & frame_a)
+ { frame_a.derived() = getFrameAcceleration(model, data, frame_id); }
/**
* @brief Returns the jacobian of the frame expressed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
@@ -131,23 +194,6 @@ namespace pinocchio
const FrameIndex frameId,
const Eigen::MatrixBase<Matrix6Like> & J);
- /**
- * @brief Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
- * You must first call pinocchio::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
- */
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MotionLike>
- void getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- const DataTpl<Scalar,Options,JointCollectionTpl> & data,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
- const MotionDense<MotionLike> & frame_a);
-
/**
* @brief Returns the jacobian of the frame expresssed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
* depending on the value of rf.
@@ -155,7 +201,7 @@ namespace pinocchio
*
* @tparam rf Reference frame in which the columns of the Jacobian are expressed.
* @deprecated This function is now deprecated. Please call pinocchio::getFrameJacobian for same functionality.
-
+ *
* @remark Similarly to pinocchio::getJointJacobian with LOCAL or WORLD 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
* and expressed in a coordinate system aligned with the WORLD.
diff --git a/src/algorithm/frames.hxx b/src/algorithm/frames.hxx
index 2441fadb2..d2dbeb64a 100644
--- a/src/algorithm/frames.hxx
+++ b/src/algorithm/frames.hxx
@@ -74,11 +74,11 @@ namespace pinocchio
updateFramePlacements(model, data);
}
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MotionLike>
- void getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- const DataTpl<Scalar,Options,JointCollectionTpl> & data,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
- const MotionDense<MotionLike> & frame_v)
+ template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+ inline MotionTpl<Scalar, Options>
+ getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+ const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+ const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id)
{
assert(model.check(data) && "data is not consistent with model.");
@@ -86,14 +86,14 @@ namespace pinocchio
const typename Model::Frame & frame = model.frames[frame_id];
const typename Model::JointIndex & parent = frame.parent;
- const_cast<MotionLike &>(frame_v.derived()) = frame.placement.actInv(data.v[parent]);
- }
+ return frame.placement.actInv(data.v[parent]);
+ }
- template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MotionLike>
- void getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
- const DataTpl<Scalar,Options,JointCollectionTpl> & data,
- const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
- const MotionDense<MotionLike> & frame_a)
+ template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+ inline MotionTpl<Scalar, Options>
+ getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+ const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+ const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id)
{
assert(model.check(data) && "data is not consistent with model.");
@@ -101,7 +101,7 @@ namespace pinocchio
const typename Model::Frame & frame = model.frames[frame_id];
const typename Model::JointIndex & parent = frame.parent;
- const_cast<MotionLike &>(frame_a.derived()) = frame.placement.actInv(data.a[parent]);
+ return frame.placement.actInv(data.a[parent]);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
diff --git a/unittest/frames.cpp b/unittest/frames.cpp
index d45b1f769..9cc115713 100644
--- a/unittest/frames.cpp
+++ b/unittest/frames.cpp
@@ -148,8 +148,7 @@ BOOST_AUTO_TEST_CASE ( test_velocity )
VectorXd v = VectorXd::Ones(model.nv);
forwardKinematics(model, data, q, v);
- Motion vf;
- getFrameVelocity(model, data, frame_idx, vf);
+ Motion vf = getFrameVelocity(model, data, frame_idx);
BOOST_CHECK(vf.isApprox(framePlacement.actInv(data.v[parent_idx])));
}
@@ -174,8 +173,7 @@ BOOST_AUTO_TEST_CASE ( test_acceleration )
VectorXd a = VectorXd::Ones(model.nv);
forwardKinematics(model, data, q, v, a);
- Motion af;
- getFrameAcceleration(model, data, frame_idx, af);
+ Motion af = getFrameAcceleration(model, data, frame_idx);
BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx])));
}
--
GitLab