Commit 4a934ff5 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Implement specific difference for SE(3) and SE(2) joints

  - difference is computed for R2xSO(2) and R3xSO(3).
parent b7898000
......@@ -89,6 +89,10 @@ namespace hpp {
/// \f$\textbf{v}\f$
/// \note If the configuration space is a vector space, this is
/// \f$\textbf{v} = q_1 - q_2\f$
template <typename LieGroup>
void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, vectorOut_t result);
void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, vectorOut_t result);
......
......@@ -82,14 +82,26 @@ namespace hpp {
interpolate<LieGroupTpl> (robot, q0, q1, u, result);
}
template <typename LieGroup>
void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, vectorOut_t result)
{
result = se3::differentiate<se3::LieGroupTpl>(robot->model(), q2, q1);
result = se3::differentiate<LieGroup> (robot->model(), q2, q1);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = q1.tail (dim) - q2.tail (dim);
}
template void difference <se3::LieGroupTpl> (const DevicePtr_t& robot,
ConfigurationIn_t q1,
ConfigurationIn_t q2,
vectorOut_t result);
void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, vectorOut_t result)
{
difference <LieGroupTpl> (robot, q1, q2, result);
}
bool isApprox (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, value_type eps)
{
......
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