Commit 73a23831 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Interpolate, integrate and difference takes into account extra dofs

parent 620a4f1c
...@@ -53,6 +53,8 @@ namespace hpp { ...@@ -53,6 +53,8 @@ namespace hpp {
indexConfig, indexVelocity, indexConfig, indexVelocity,
result); result);
} }
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = configuration.tail (dim) + velocity.tail (dim);
} }
/// Interpolate between two configurations of the robot /// Interpolate between two configurations of the robot
...@@ -74,6 +76,8 @@ namespace hpp { ...@@ -74,6 +76,8 @@ namespace hpp {
(*itJoint)->configuration ()->interpolate (*itJoint)->configuration ()->interpolate
(q0, q1, u, indexConfig, result); (q0, q1, u, indexConfig, result);
} }
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = u * q1.tail (dim) + (1-u) * q0.tail (dim);
} }
/// Difference between two configurations as a vector /// Difference between two configurations as a vector
...@@ -95,6 +99,8 @@ namespace hpp { ...@@ -95,6 +99,8 @@ namespace hpp {
(*itJoint)->configuration ()->difference (q1, q2, indexConfig, (*itJoint)->configuration ()->difference (q1, q2, indexConfig,
indexVelocity, result); indexVelocity, result);
} }
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = q2.tail (dim) - q1.tail (dim);
} }
/// Normalize configuration /// Normalize configuration
......
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