Fixed bug in RobotVelocity, in which linear and angular velocity comp…

…onents where swapped when converting from franka::CartesianVelocities
TimSchneider42 · Jun 4, 2024 · 72f15ea · 72f15ea
1 parent 67e690f
commit 72f15ea
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/src/robot_velocity.cpp b/src/robot_velocity.cpp
@@ -23,8 +23,8 @@ RobotVelocity::RobotVelocity(const Vector7d &vector_repr)
 RobotVelocity::RobotVelocity(const franka::CartesianVelocities franka_velocity)
     : RobotVelocity(
     Twist{
-        Vector6d::Map(franka_velocity.O_dP_EE.data()).tail<3>(),
-        Vector6d::Map(franka_velocity.O_dP_EE.data()).head<3>()
+        Vector6d::Map(franka_velocity.O_dP_EE.data()).head<3>(),
+        Vector6d::Map(franka_velocity.O_dP_EE.data()).tail<3>()
     }, franka_velocity.elbow[0]) {}
 
 Vector7d RobotVelocity::vector_repr() const {