From e185aa9cc446581f61e86b097b82434ba642c272 Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Mon, 10 Dec 2018 14:36:59 -0200 Subject: [PATCH 1/3] fix reference frame in exported velocities and accelerations The specification of the target frame, when the link is exported, did not have any effect on the acceleration/velocity frame. This is obviously very inconsistent. However, this is a VERY breaking change. It's hidden behind a flag (link_export_use_proper_references) which is currently false. --- tasks/ModelTask.cpp | 78 ++++++++++++++++++++++++++++++--------------- 1 file changed, 53 insertions(+), 25 deletions(-) diff --git a/tasks/ModelTask.cpp b/tasks/ModelTask.cpp index ebe35a0..9cb4ca6 100644 --- a/tasks/ModelTask.cpp +++ b/tasks/ModelTask.cpp @@ -49,7 +49,7 @@ void ModelTask::setGazeboModel(WorldPtr _world, ModelPtr _model) _model_frame.set(_model->GetName()); if (_world_frame.get().empty()) _world_frame.set(GzGet((*_world), Name, ())); -} +} void ModelTask::InternalJointExport::addJoint(JointPtr joint, std::string name) { @@ -81,12 +81,12 @@ void ModelTask::setupJoints() } #endif gzmsg << "ModelTask: found joint: " << GzGet((*world), Name, ()) + "/" + model->GetName() + - "/" + joint->GetName() << endl; + "/" + joint->GetName() << endl; main_joint_export.addJoint(joint, joint->GetScopedName()); } exported_joints.push_back(main_joint_export); - + std::vector requested_exports = _exported_joints.get(); @@ -252,16 +252,16 @@ void ModelTask::updateModelPose(base::Time const& time) rbs.sourceFrame = _model_frame.get(); rbs.targetFrame = _world_frame.get(); rbs.position = base::Vector3d( - model2world.Pos().X(),model2world.Pos().Y(),model2world.Pos().Z()); + model2world.Pos().X(),model2world.Pos().Y(),model2world.Pos().Z()); rbs.cov_position = _cov_position.get(); rbs.orientation = base::Quaterniond( - model2world.Rot().W(),model2world.Rot().X(),model2world.Rot().Y(),model2world.Rot().Z() ); + model2world.Rot().W(),model2world.Rot().X(),model2world.Rot().Y(),model2world.Rot().Z() ); rbs.cov_orientation = _cov_orientation.get(); rbs.velocity = base::Vector3d( - model2world_vel.X(), model2world_vel.Y(), model2world_vel.Z()); + model2world_vel.X(), model2world_vel.Y(), model2world_vel.Z()); rbs.cov_velocity = _cov_velocity.get(); rbs.angular_velocity = base::Vector3d( - model2world_angular_vel.X(), model2world_angular_vel.Y(), model2world_angular_vel.Z()); + model2world_angular_vel.X(), model2world_angular_vel.Y(), model2world_angular_vel.Z()); _pose_samples.write(rbs); } @@ -337,7 +337,7 @@ void ModelTask::updateLinks(base::Time const& time) for(auto& exported_link : link_export_setup) { //do not update the link if the last port writing happened - //in less then link_period. + //in less then link_period. if (!(exported_link.last_update.isNull())) { if ((time - exported_link.last_update) <= exported_link.port_period) @@ -346,40 +346,59 @@ void ModelTask::updateLinks(base::Time const& time) Pose3d source2world = Pose3d::Zero; Vector3d sourceInWorld_linear_vel = Vector3d::Zero; - Vector3d source_angular_vel = Vector3d::Zero; + Vector3d sourceInWorld_angular_vel = Vector3d::Zero; Vector3d sourceInWorld_linear_acc = Vector3d::Zero; - Vector3d source_angular_acc = Vector3d::Zero; + Vector3d sourceInWorld_angular_acc = Vector3d::Zero; if (exported_link.source_link_ptr) { source2world = GzGetIgn((*(exported_link.source_link_ptr)), WorldPose, ()); - sourceInWorld_linear_vel = GzGetIgn((*(exported_link.source_link_ptr)), WorldLinearVel, ()); - source_angular_vel = GzGetIgn((*(exported_link.source_link_ptr)), RelativeAngularVel, ()); - sourceInWorld_linear_acc = GzGetIgn((*(exported_link.source_link_ptr)), WorldLinearAccel, ()); - source_angular_acc = GzGetIgn((*(exported_link.source_link_ptr)), RelativeAngularAccel, ()); + sourceInWorld_linear_vel = + GzGetIgn((*(exported_link.source_link_ptr)), WorldLinearVel, ()); + sourceInWorld_angular_vel = + GzGetIgn((*(exported_link.source_link_ptr)), WorldAngularVel, ()); + sourceInWorld_linear_acc = + GzGetIgn((*(exported_link.source_link_ptr)), WorldLinearAccel, ()); + sourceInWorld_angular_acc = + GzGetIgn((*(exported_link.source_link_ptr)), WorldAngularAccel, ()); } Pose3d target2world = Pose3d::Zero; if (exported_link.target_link_ptr) - target2world = GzGetIgn((*(exported_link.target_link_ptr)), WorldPose, ()); + target2world = GzGetIgn((*(exported_link.target_link_ptr)), WorldPose, ()); Pose3d source2target( Pose3d(source2world - target2world) ); - Vector3d sourceInTarget_linear_vel (target2world.Rot().RotateVectorReverse(sourceInWorld_linear_vel)); - Vector3d sourceInTarget_linear_acc (target2world.Rot().RotateVectorReverse(sourceInWorld_linear_acc)); + Vector3d sourceInTarget_linear_vel = + target2world.Rot().RotateVectorReverse(sourceInWorld_linear_vel); + Vector3d sourceInTarget_linear_acc = + target2world.Rot().RotateVectorReverse(sourceInWorld_linear_acc); + Vector3d sourceInTarget_angular_vel = + target2world.Rot().RotateVectorReverse(sourceInWorld_angular_vel); + Vector3d sourceInTarget_angular_acc = + target2world.Rot().RotateVectorReverse(sourceInWorld_angular_acc); RigidBodyState rbs; rbs.sourceFrame = exported_link.source_frame; rbs.targetFrame = exported_link.target_frame; rbs.position = base::Vector3d( - source2target.Pos().X(),source2target.Pos().Y(),source2target.Pos().Z()); + source2target.Pos().X(), + source2target.Pos().Y(), + source2target.Pos().Z()); rbs.cov_position = exported_link.cov_position; rbs.orientation = base::Quaterniond( - source2target.Rot().W(),source2target.Rot().X(),source2target.Rot().Y(),source2target.Rot().Z() ); + source2target.Rot().W(), + source2target.Rot().X(), + source2target.Rot().Y(), + source2target.Rot().Z() ); rbs.cov_orientation = exported_link.cov_orientation; rbs.velocity = base::Vector3d( - sourceInTarget_linear_vel.X(),sourceInTarget_linear_vel.Y(),sourceInTarget_linear_vel.Z()); + sourceInTarget_linear_vel.X(), + sourceInTarget_linear_vel.Y(), + sourceInTarget_linear_vel.Z()); rbs.cov_velocity = exported_link.cov_velocity; rbs.angular_velocity = base::Vector3d( - source_angular_vel.X(),source_angular_vel.Y(),source_angular_vel.Z()); + sourceInTarget_angular_vel.X(), + sourceInTarget_angular_vel.Y(), + sourceInTarget_angular_vel.Z()); rbs.cov_angular_velocity = exported_link.cov_angular_velocity; rbs.time = time; exported_link.port->write(rbs); @@ -387,9 +406,13 @@ void ModelTask::updateLinks(base::Time const& time) base::samples::RigidBodyAcceleration rba; rba.cov_acceleration = exported_link.cov_acceleration; rba.acceleration = base::Vector3d( - sourceInTarget_linear_acc.X(),sourceInTarget_linear_acc.Y(),sourceInTarget_linear_acc.Z()); + sourceInTarget_linear_acc.X(), + sourceInTarget_linear_acc.Y(), + sourceInTarget_linear_acc.Z()); rba.angular_acceleration = base::Vector3d( - source_angular_acc.X(),source_angular_acc.Y(),source_angular_acc.Z()); + sourceInTarget_angular_acc.X(), + sourceInTarget_angular_acc.Y(), + sourceInTarget_angular_acc.Z()); rba.cov_angular_acceleration = exported_link.cov_angular_acceleration; rba.time = time; exported_link.rba_port->write(rba); @@ -415,9 +438,14 @@ void ModelTask::updateLinks(base::Time const& time) Pose3d source2world = GzGetIgn((*exported_link.source_link_ptr), WorldPose, ()); exported_link.source_link_ptr->SetForce(source2world.Rot().RotateVector( - Vector3d(exported_link.wrench_in.force[0], exported_link.wrench_in.force[1], exported_link.wrench_in.force[2]))); + Vector3d(exported_link.wrench_in.force[0], + exported_link.wrench_in.force[1], + exported_link.wrench_in.force[2]))); exported_link.source_link_ptr->SetTorque(source2world.Rot().RotateVector( - Vector3d(exported_link.wrench_in.torque[0], exported_link.wrench_in.torque[1], exported_link.wrench_in.torque[2]))); + Vector3d( + exported_link.wrench_in.torque[0], + exported_link.wrench_in.torque[1], + exported_link.wrench_in.torque[2]))); } } From 865b9f5e9666fab6f64d41234bb08e9b7756e5d6 Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Mon, 10 Dec 2018 14:51:16 -0200 Subject: [PATCH 2/3] hide the new reference frame behavior under a flag This is too breaking a change to be left alone like this. --- rock_gazebo.orogen | 7 +++++++ tasks/ModelTask.cpp | 43 +++++++++++++++++++++++-------------------- 2 files changed, 30 insertions(+), 20 deletions(-) diff --git a/rock_gazebo.orogen b/rock_gazebo.orogen index 4f19c13..4520244 100644 --- a/rock_gazebo.orogen +++ b/rock_gazebo.orogen @@ -29,6 +29,13 @@ end task_context "ModelTask", subclasses: 'BaseTask' do needs_configuration + # rock-gazebo was initially exporting velocities and accelerations in + # the body reference frame. Set this to true to switch to a consistent + # reference frame for all velocities and accelerations + # + # Note that true will become the default in the future. + property 'use_proper_reference_frames', 'bool', false + # How long will the task consider that a value received on the joints_cmd # port is valid. # diff --git a/tasks/ModelTask.cpp b/tasks/ModelTask.cpp index 9cb4ca6..6afe8f0 100644 --- a/tasks/ModelTask.cpp +++ b/tasks/ModelTask.cpp @@ -367,14 +367,17 @@ void ModelTask::updateLinks(base::Time const& time) target2world = GzGetIgn((*(exported_link.target_link_ptr)), WorldPose, ()); Pose3d source2target( Pose3d(source2world - target2world) ); - Vector3d sourceInTarget_linear_vel = - target2world.Rot().RotateVectorReverse(sourceInWorld_linear_vel); - Vector3d sourceInTarget_linear_acc = - target2world.Rot().RotateVectorReverse(sourceInWorld_linear_acc); - Vector3d sourceInTarget_angular_vel = - target2world.Rot().RotateVectorReverse(sourceInWorld_angular_vel); - Vector3d sourceInTarget_angular_acc = - target2world.Rot().RotateVectorReverse(sourceInWorld_angular_acc); + + auto velacc_reference_rot = _use_proper_reference_frames.get() ? + target2world.Rot() : source2world.Rot(); + Vector3d linear_vel = + velacc_reference_rot.RotateVectorReverse(sourceInWorld_linear_vel); + Vector3d linear_acc = + velacc_reference_rot.RotateVectorReverse(sourceInWorld_linear_acc); + Vector3d angular_vel = + velacc_reference_rot.RotateVectorReverse(sourceInWorld_angular_vel); + Vector3d angular_acc = + velacc_reference_rot.RotateVectorReverse(sourceInWorld_angular_acc); RigidBodyState rbs; rbs.sourceFrame = exported_link.source_frame; @@ -391,14 +394,14 @@ void ModelTask::updateLinks(base::Time const& time) source2target.Rot().Z() ); rbs.cov_orientation = exported_link.cov_orientation; rbs.velocity = base::Vector3d( - sourceInTarget_linear_vel.X(), - sourceInTarget_linear_vel.Y(), - sourceInTarget_linear_vel.Z()); + linear_vel.X(), + linear_vel.Y(), + linear_vel.Z()); rbs.cov_velocity = exported_link.cov_velocity; rbs.angular_velocity = base::Vector3d( - sourceInTarget_angular_vel.X(), - sourceInTarget_angular_vel.Y(), - sourceInTarget_angular_vel.Z()); + angular_vel.X(), + angular_vel.Y(), + angular_vel.Z()); rbs.cov_angular_velocity = exported_link.cov_angular_velocity; rbs.time = time; exported_link.port->write(rbs); @@ -406,13 +409,13 @@ void ModelTask::updateLinks(base::Time const& time) base::samples::RigidBodyAcceleration rba; rba.cov_acceleration = exported_link.cov_acceleration; rba.acceleration = base::Vector3d( - sourceInTarget_linear_acc.X(), - sourceInTarget_linear_acc.Y(), - sourceInTarget_linear_acc.Z()); + linear_acc.X(), + linear_acc.Y(), + linear_acc.Z()); rba.angular_acceleration = base::Vector3d( - sourceInTarget_angular_acc.X(), - sourceInTarget_angular_acc.Y(), - sourceInTarget_angular_acc.Z()); + angular_acc.X(), + angular_acc.Y(), + angular_acc.Z()); rba.cov_angular_acceleration = exported_link.cov_angular_acceleration; rba.time = time; exported_link.rba_port->write(rba); From e0a6876a33081f2a0ecb81fa9ef2beb2e28b39c0 Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Thu, 13 Dec 2018 16:01:49 -0200 Subject: [PATCH 3/3] remove use_proper_reference_frames to match the RBS and RBA definitions The documentation is clear (even if confusing), so make sure that we're following that unconditionally --- rock_gazebo.orogen | 7 ------- tasks/ModelTask.cpp | 10 ++++------ 2 files changed, 4 insertions(+), 13 deletions(-) diff --git a/rock_gazebo.orogen b/rock_gazebo.orogen index 4520244..4f19c13 100644 --- a/rock_gazebo.orogen +++ b/rock_gazebo.orogen @@ -29,13 +29,6 @@ end task_context "ModelTask", subclasses: 'BaseTask' do needs_configuration - # rock-gazebo was initially exporting velocities and accelerations in - # the body reference frame. Set this to true to switch to a consistent - # reference frame for all velocities and accelerations - # - # Note that true will become the default in the future. - property 'use_proper_reference_frames', 'bool', false - # How long will the task consider that a value received on the joints_cmd # port is valid. # diff --git a/tasks/ModelTask.cpp b/tasks/ModelTask.cpp index 6afe8f0..a1efc7d 100644 --- a/tasks/ModelTask.cpp +++ b/tasks/ModelTask.cpp @@ -368,16 +368,14 @@ void ModelTask::updateLinks(base::Time const& time) Pose3d source2target( Pose3d(source2world - target2world) ); - auto velacc_reference_rot = _use_proper_reference_frames.get() ? - target2world.Rot() : source2world.Rot(); Vector3d linear_vel = - velacc_reference_rot.RotateVectorReverse(sourceInWorld_linear_vel); + target2world.Rot().RotateVectorReverse(sourceInWorld_linear_vel); Vector3d linear_acc = - velacc_reference_rot.RotateVectorReverse(sourceInWorld_linear_acc); + target2world.Rot().RotateVectorReverse(sourceInWorld_linear_acc); Vector3d angular_vel = - velacc_reference_rot.RotateVectorReverse(sourceInWorld_angular_vel); + source2world.Rot().RotateVectorReverse(sourceInWorld_angular_vel); Vector3d angular_acc = - velacc_reference_rot.RotateVectorReverse(sourceInWorld_angular_acc); + target2world.Rot().RotateVectorReverse(sourceInWorld_angular_acc); RigidBodyState rbs; rbs.sourceFrame = exported_link.source_frame;