Commit 94538b5d authored by jcarpent's avatar jcarpent
Browse files

[All] Change differentiate into difference

Differentiate was not the right term as it is devoded to differential calculus.
This is related to #432.
parent 725f4ffb
...@@ -50,8 +50,8 @@ namespace se3 ...@@ -50,8 +50,8 @@ namespace se3
"Double u"), "Double u"),
"Interpolate the model between two configurations."); "Interpolate the model between two configurations.");
bp::def("differentiate", bp::def("difference",
(VectorXd (*)(const Model &, const VectorXd &, const VectorXd &))&differentiate, (VectorXd (*)(const Model &, const VectorXd &, const VectorXd &))&difference,
bp::args("Model", bp::args("Model",
"Configuration q1 (size Model::nq)", "Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)"), "Configuration q2 (size Model::nq)"),
......
...@@ -53,4 +53,6 @@ def getJacobianTimeVariation(model,data,jointId,local): ...@@ -53,4 +53,6 @@ def getJacobianTimeVariation(model,data,jointId,local):
else: else:
return se3.getJointJacobianTimeVariation(model,data,jointId,se3.ReferenceFrame.WORLD) return se3.getJointJacobianTimeVariation(model,data,jointId,se3.ReferenceFrame.WORLD)
@deprecated("This function has been renamed difference and will be removed in release 1.4.0 of Pinocchio. Please change for new difference.")
def differentiate(model,q0,q1):
return difference(model,q0,q1)
...@@ -54,15 +54,15 @@ namespace se3 ...@@ -54,15 +54,15 @@ namespace se3
/** /**
* @brief Compute the tangent vector that must be integrated during one unit time to go from q0 to q1 * @brief Compute the tangent vector that must be integrated during one unit time to go from q0 to q1
* *
* @param[in] model Model to be differentiated * @param[in] model Model to be differenced
* @param[in] q0 Initial configuration (size model.nq) * @param[in] q0 Initial configuration (size model.nq)
* @param[in] q1 Wished configuration (size model.nq) * @param[in] q1 Wished configuration (size model.nq)
* @return The corresponding velocity (size model.nv) * @return The corresponding velocity (size model.nv)
*/ */
template<typename LieGroup_t> template<typename LieGroup_t>
inline Eigen::VectorXd differentiate(const Model & model, inline Eigen::VectorXd difference(const Model & model,
const Eigen::VectorXd & q0, const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1); const Eigen::VectorXd & q1);
/** /**
......
...@@ -80,25 +80,25 @@ namespace se3 ...@@ -80,25 +80,25 @@ namespace se3
template<typename LieGroup_t> template<typename LieGroup_t>
inline Eigen::VectorXd inline Eigen::VectorXd
differentiate(const Model & model, difference(const Model & model,
const Eigen::VectorXd & q0, const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1) const Eigen::VectorXd & q1)
{ {
Eigen::VectorXd diff(model.nv); Eigen::VectorXd diff(model.nv);
typename DifferentiateStep<LieGroup_t>::ArgsType args(q0, q1, diff); typename DifferenceStep<LieGroup_t>::ArgsType args(q0, q1, diff);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i ) for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{ {
DifferentiateStep<LieGroup_t>::run(model.joints[i], args); DifferenceStep<LieGroup_t>::run(model.joints[i], args);
} }
return diff; return diff;
} }
inline Eigen::VectorXd inline Eigen::VectorXd
differentiate(const Model & model, difference(const Model & model,
const Eigen::VectorXd & q0, const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1) const Eigen::VectorXd & q1)
{ {
return differentiate<LieGroupMap>(model, q0, q1); return difference<LieGroupMap>(model, q0, q1);
} }
template<typename LieGroup_t> template<typename LieGroup_t>
......
...@@ -157,23 +157,23 @@ namespace se3 ...@@ -157,23 +157,23 @@ namespace se3
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(InterpolateStep, InterpolateStepAlgo); SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(InterpolateStep, InterpolateStepAlgo);
template<typename LieGroup_t, typename JointModel> struct DifferentiateStepAlgo; template<typename LieGroup_t, typename JointModel> struct DifferenceStepAlgo;
template<typename LieGroup_t> template<typename LieGroup_t>
struct DifferentiateStep : public fusion::JointModelVisitor<DifferentiateStep<LieGroup_t> > struct DifferenceStep : public fusion::JointModelVisitor<DifferenceStep<LieGroup_t> >
{ {
typedef boost::fusion::vector<const Eigen::VectorXd &, typedef boost::fusion::vector<const Eigen::VectorXd &,
const Eigen::VectorXd &, const Eigen::VectorXd &,
Eigen::VectorXd & Eigen::VectorXd &
> ArgsType; > ArgsType;
JOINT_MODEL_VISITOR_INIT(DifferentiateStep); JOINT_MODEL_VISITOR_INIT(DifferenceStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_3(DifferentiateStepAlgo, LieGroup_t) SE3_DETAILS_VISITOR_METHOD_ALGO_3(DifferenceStepAlgo, LieGroup_t)
}; };
template<typename LieGroup_t, typename JointModel> template<typename LieGroup_t, typename JointModel>
struct DifferentiateStepAlgo { struct DifferenceStepAlgo {
static void run(const se3::JointModelBase<JointModel> & jmodel, static void run(const se3::JointModelBase<JointModel> & jmodel,
const Eigen::VectorXd & q0, const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1, const Eigen::VectorXd & q1,
...@@ -186,7 +186,7 @@ namespace se3 ...@@ -186,7 +186,7 @@ namespace se3
} }
}; };
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(DifferentiateStep, DifferentiateStepAlgo); SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(DifferenceStep, DifferenceStepAlgo);
template<typename LieGroup_t, typename JointModel> struct SquaredDistanceStepAlgo; template<typename LieGroup_t, typename JointModel> struct SquaredDistanceStepAlgo;
......
...@@ -100,9 +100,9 @@ BOOST_AUTO_TEST_CASE ( integrate_difference_test ) ...@@ -100,9 +100,9 @@ BOOST_AUTO_TEST_CASE ( integrate_difference_test )
Eigen::VectorXd q1(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) )); Eigen::VectorXd q1(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
Eigen::VectorXd qdot(Eigen::VectorXd::Random(model.nv)); Eigen::VectorXd qdot(Eigen::VectorXd::Random(model.nv));
BOOST_CHECK_MESSAGE(isSameConfiguration(model, integrate(model, q0, differentiate(model, q0,q1)), q1), "Integrate (differentiate) - wrong results"); BOOST_CHECK_MESSAGE(isSameConfiguration(model, integrate(model, q0, difference(model, q0,q1)), q1), "Integrate (difference) - wrong results");
BOOST_CHECK_MESSAGE(differentiate(model, q0, integrate(model,q0, qdot)).isApprox(qdot),"differentiate (integrate) - wrong results"); BOOST_CHECK_MESSAGE(difference(model, q0, integrate(model,q0, qdot)).isApprox(qdot),"difference (integrate) - wrong results");
} }
...@@ -137,7 +137,7 @@ BOOST_AUTO_TEST_CASE ( distance_configuration_test ) ...@@ -137,7 +137,7 @@ BOOST_AUTO_TEST_CASE ( distance_configuration_test )
double dist = distance(model,q0,q1); double dist = distance(model,q0,q1);
BOOST_CHECK_MESSAGE(dist > 0., "distance - wrong results"); BOOST_CHECK_MESSAGE(dist > 0., "distance - wrong results");
BOOST_CHECK_SMALL(dist-differentiate(model,q0,q1).norm(), 1e-12); BOOST_CHECK_SMALL(dist-difference(model,q0,q1).norm(), 1e-12);
} }
BOOST_AUTO_TEST_CASE ( uniform_sampling_test ) BOOST_AUTO_TEST_CASE ( uniform_sampling_test )
......
...@@ -105,7 +105,7 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &) ...@@ -105,7 +105,7 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &)
zero = LieGroupType().difference(q2,q2); zero = LieGroupType().difference(q2,q2);
BOOST_CHECK_MESSAGE (zero.isZero (0), std::string ("Error: difference between two equal configurations is not 0.")); BOOST_CHECK_MESSAGE (zero.isZero (0), std::string ("Error: difference between two equal configurations is not 0."));
// Check differentiate // Check difference
TangentVector_t vdiff = LieGroupType().difference(q1,q2); TangentVector_t vdiff = LieGroupType().difference(q1,q2);
BOOST_CHECK_MESSAGE(vdiff.isApprox(q1_dot,1e2*prec), std::string("Error when differentiating " + jmodel.shortname())); BOOST_CHECK_MESSAGE(vdiff.isApprox(q1_dot,1e2*prec), std::string("Error when differentiating " + jmodel.shortname()));
......
Supports Markdown
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