From 7f0e4e56cb01a9b064ef353f456e658c4ac6378f Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Fri, 30 Dec 2016 21:45:20 -0200 Subject: [PATCH 1/2] output velocity and angular velocity in a ModelTask's link export --- tasks/ModelTask.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/tasks/ModelTask.cpp b/tasks/ModelTask.cpp index aeac38d..2d722bb 100644 --- a/tasks/ModelTask.cpp +++ b/tasks/ModelTask.cpp @@ -243,20 +243,37 @@ void ModelTask::updateLinks(base::Time const& time) } math::Pose source2world = math::Pose::Zero; + math::Vector3 sourceInWorld_linear_vel = math::Vector3::Zero; + math::Vector3 sourceInWorld_angular_vel = math::Vector3::Zero; if (it->second.source_link_ptr) + { source2world = it->second.source_link_ptr->GetWorldPose(); + sourceInWorld_linear_vel = it->second.source_link_ptr->GetWorldLinearVel(); + sourceInWorld_angular_vel = it->second.source_link_ptr->GetWorldAngularVel(); + } + math::Pose target2world = math::Pose::Zero; if (it->second.target_link_ptr) target2world = it->second.target_link_ptr->GetWorldPose(); + math::Pose source2target( math::Pose(source2world - target2world) ); + math::Vector3 sourceInTarget_angular_vel(target2world.rot.RotateVectorReverse(sourceInWorld_angular_vel)); + math::Vector3 sourceInTarget_linear_vel (target2world.rot.RotateVectorReverse(sourceInWorld_linear_vel)); RigidBodyState rbs; rbs.sourceFrame = it->second.source_frame; rbs.targetFrame = it->second.target_frame; rbs.position = base::Vector3d( source2target.pos.x,source2target.pos.y,source2target.pos.z); + rbs.cov_position = _cov_position; rbs.orientation = base::Quaterniond( source2target.rot.w,source2target.rot.x,source2target.rot.y,source2target.rot.z ); + rbs.cov_orientation = _cov_orientation; + rbs.velocity = base::Vector3d( + sourceInTarget_linear_vel.x,sourceInTarget_linear_vel.y,sourceInTarget_linear_vel.z); + rbs.cov_velocity = _cov_velocity; + rbs.angular_velocity = base::Vector3d( + sourceInTarget_angular_vel.x,sourceInTarget_angular_vel.y,sourceInTarget_angular_vel.z); rbs.time = time; it->second.port->write(rbs); it->second.last_update = time; From ee87c85934c67c87d285602243fcc7ead3dfbff7 Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Fri, 6 Jan 2017 12:39:15 -0200 Subject: [PATCH 2/2] generate body-fixed angular velocities as documented for RigidBodyState --- tasks/ModelTask.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tasks/ModelTask.cpp b/tasks/ModelTask.cpp index 2d722bb..ab9d24b 100644 --- a/tasks/ModelTask.cpp +++ b/tasks/ModelTask.cpp @@ -244,12 +244,12 @@ void ModelTask::updateLinks(base::Time const& time) math::Pose source2world = math::Pose::Zero; math::Vector3 sourceInWorld_linear_vel = math::Vector3::Zero; - math::Vector3 sourceInWorld_angular_vel = math::Vector3::Zero; + math::Vector3 source_angular_vel; if (it->second.source_link_ptr) { source2world = it->second.source_link_ptr->GetWorldPose(); sourceInWorld_linear_vel = it->second.source_link_ptr->GetWorldLinearVel(); - sourceInWorld_angular_vel = it->second.source_link_ptr->GetWorldAngularVel(); + source_angular_vel = it->second.source_link_ptr->GetRelativeAngularVel(); } math::Pose target2world = math::Pose::Zero; @@ -257,7 +257,6 @@ void ModelTask::updateLinks(base::Time const& time) target2world = it->second.target_link_ptr->GetWorldPose(); math::Pose source2target( math::Pose(source2world - target2world) ); - math::Vector3 sourceInTarget_angular_vel(target2world.rot.RotateVectorReverse(sourceInWorld_angular_vel)); math::Vector3 sourceInTarget_linear_vel (target2world.rot.RotateVectorReverse(sourceInWorld_linear_vel)); RigidBodyState rbs; @@ -273,7 +272,7 @@ void ModelTask::updateLinks(base::Time const& time) sourceInTarget_linear_vel.x,sourceInTarget_linear_vel.y,sourceInTarget_linear_vel.z); rbs.cov_velocity = _cov_velocity; rbs.angular_velocity = base::Vector3d( - sourceInTarget_angular_vel.x,sourceInTarget_angular_vel.y,sourceInTarget_angular_vel.z); + source_angular_vel.x,source_angular_vel.y,source_angular_vel.z); rbs.time = time; it->second.port->write(rbs); it->second.last_update = time;