Commit 783b2465 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Homogenize jacobians and use of velocities.

parent 5b173618
......@@ -66,12 +66,19 @@ namespace hpp {
/// \param u in [0,1] position along the interpolation: q0 for u=0,
/// q1 for u=1
/// \retval result interpolated configuration
template <typename LieGroup>
void interpolate (const DevicePtr_t& robot,
ConfigurationIn_t q0,
ConfigurationIn_t q1,
const value_type& u,
ConfigurationOut_t result);
void interpolate (const DevicePtr_t& robot,
ConfigurationIn_t q0,
ConfigurationIn_t q1,
const value_type& u,
ConfigurationOut_t result);
/// Difference between two configurations as a vector
///
/// \param robot robot that describes the kinematic chain
......
......@@ -17,6 +17,7 @@
#include <hpp/pinocchio/configuration.hh>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/multibody/liegroup/liegroup.hpp>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/liegroup.hh>
......@@ -41,7 +42,7 @@ namespace hpp {
vectorIn_t velocity, ConfigurationOut_t result)
{
const se3::Model& model = robot->model();
result.head(model.nq) = se3::integrate<LieGroupTpl>(model, configuration, velocity);
result.head(model.nq) = se3::integrate<se3::LieGroupTpl>(model, configuration, velocity);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = configuration.tail (dim) + velocity.tail (dim);
if (saturateConfig) saturate(robot, result);
......@@ -54,21 +55,37 @@ namespace hpp {
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result);
template <typename LieGroup>
void interpolate (const DevicePtr_t& robot,
ConfigurationIn_t q0,
ConfigurationIn_t q1,
const value_type& u,
ConfigurationOut_t result)
{
result = se3::interpolate<LieGroupTpl>(robot->model(), q0, q1, u);
result = se3::interpolate<LieGroup>(robot->model(), q0, q1, u);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = u * q1.tail (dim) + (1-u) * q0.tail (dim);
}
template void interpolate<se3::LieGroupTpl> (const DevicePtr_t& robot,
ConfigurationIn_t q0,
ConfigurationIn_t q1,
const value_type& u,
ConfigurationOut_t result);
void interpolate (const DevicePtr_t& robot,
ConfigurationIn_t q0,
ConfigurationIn_t q1,
const value_type& u,
ConfigurationOut_t result)
{
interpolate<LieGroupTpl> (robot, q0, q1, u, result);
}
void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, vectorOut_t result)
{
result = se3::differentiate<LieGroupTpl>(robot->model(), q2, q1);
result = se3::differentiate<se3::LieGroupTpl>(robot->model(), q2, q1);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = q1.tail (dim) - q2.tail (dim);
}
......@@ -76,7 +93,7 @@ namespace hpp {
bool isApprox (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, value_type eps)
{
if (!se3::isSameConfiguration<LieGroupTpl>(robot->model(), q1, q2, eps)) return false;
if (!se3::isSameConfiguration<se3::LieGroupTpl>(robot->model(), q1, q2, eps)) return false;
const size_type& dim = robot->extraConfigSpace().dimension();
return q2.tail (dim).isApprox (q1.tail (dim), eps);
}
......@@ -84,7 +101,7 @@ namespace hpp {
value_type distance (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2)
{
vector_t dist = se3::squaredDistance<LieGroupTpl>(robot->model(), q1, q2);
vector_t dist = se3::squaredDistance<se3::LieGroupTpl>(robot->model(), q1, q2);
const size_type& dim = robot->extraConfigSpace().dimension();
if (dim == 0) return sqrt(dist.sum());
else return sqrt (dist.sum() + (q2.tail (dim) - q1.tail (dim)).squaredNorm ());
......
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