Commit 35bff21f authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add template parameter to integrate

parent ad15f38a
......@@ -47,18 +47,15 @@ namespace hpp {
///
/// \note bounded degrees of freedom are saturated if the result of the
/// above operation is beyond a bound.
template<bool saturateConfig>
template<bool saturateConfig, typename LieGroup>
void integrate (const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result);
/// Same as integrate<true>
inline void integrate (const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result)
{
integrate<true> (robot, configuration, velocity, result);
}
/// Same as integrate<true, se3::LieGroupTpl>
void integrate (const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result);
/// Interpolate between two configurations of the robot
/// \param robot robot that describes the kinematic chain
......
......@@ -36,24 +36,41 @@ namespace hpp {
configuration.tail(d) = ecs.lower().cwiseMax(configuration.tail(d));
}
template<bool saturateConfig>
template<bool saturateConfig, typename LieGroup>
void integrate (const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result)
{
const se3::Model& model = robot->model();
result.head(model.nq) = se3::integrate<se3::LieGroupTpl>(model, configuration, velocity);
result.head(model.nq) = se3::integrate<LieGroup>(model, configuration, velocity);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = configuration.tail (dim) + velocity.tail (dim);
if (saturateConfig) saturate(robot, result);
}
template void integrate<true> (const DevicePtr_t& robot,
template void integrate<true, LieGroupTpl>
(const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result);
template void integrate<false>(const DevicePtr_t& robot,
template void integrate<false, LieGroupTpl>
(const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result);
template void integrate<true, se3::LieGroupTpl>
(const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result);
template void integrate<false, se3::LieGroupTpl>
(const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result);
void integrate (const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result)
{
integrate<true, se3::LieGroupTpl> (robot, configuration, velocity, result);
}
template <typename LieGroup>
void interpolate (const DevicePtr_t& robot,
......
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