From 7f0e4e56cb01a9b064ef353f456e658c4ac6378f Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Fri, 30 Dec 2016 21:45:20 -0200 Subject: [PATCH] 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;