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 {
indexConfig, indexVelocity,
result);
}
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = configuration.tail (dim) + velocity.tail (dim);
}
/// Interpolate between two configurations of the robot
......@@ -74,6 +76,8 @@ namespace hpp {
(*itJoint)->configuration ()->interpolate
(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
......@@ -95,6 +99,8 @@ namespace hpp {
(*itJoint)->configuration ()->difference (q1, q2, indexConfig,
indexVelocity, result);
}
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = q2.tail (dim) - q1.tail (dim);
}
/// Normalize configuration
......
Markdown is supported
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