output velocity and angular velocity in a ModelTask's link export

Brazilian-Institute-of-Robotics · Jan 6, 2017 · 7f0e4e5 · 7f0e4e5
1 parent 431d415
commit 7f0e4e5
Showing 1 changed file with 17 additions and 0 deletions.
diff --git 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;