Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
express the accelerations in the body frame
Browse files Browse the repository at this point in the history
This is really what makes the most sense from a sensing perspective.
Sensors (gyros, accelerometers) and models (hydrodynamic models)
give access to velocities and accelerations w.r.t. the inertial
frame, but expressed in the body frame.

However, expressing the velocities/accelerations in the world frame
from these sensing/estimation methods would require to have an estimate
of the system's pose in the world, which is a harder thing to get
doudou committed Dec 13, 2018
1 parent 3242d56 commit aead319
Showing 1 changed file with 7 additions and 8 deletions.
15 changes: 7 additions & 8 deletions tasks/ModelTask.cpp
Original file line number Diff line number Diff line change
@@ -352,10 +352,10 @@ void ModelTask::updateLinks(base::Time const& time)
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, ());
sourceRelative_angular_vel = GzGetIgn((*(exported_link.source_link_ptr)), RelativeAngularVel, ());
sourceRelative_linear_acc = GzGetIgn((*(exported_link.source_link_ptr)), RelativeLinearAccel, ());
sourceRelative_angular_acc = GzGetIgn((*(exported_link.source_link_ptr)), RelativeAngularAccel, ());
}

Pose3d target2world = Pose3d::Zero;
@@ -364,7 +364,6 @@ void ModelTask::updateLinks(base::Time const& time)

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));

RigidBodyState rbs;
rbs.sourceFrame = exported_link.source_frame;
@@ -379,17 +378,17 @@ void ModelTask::updateLinks(base::Time const& time)
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());
sourceRelative_angular_vel.X(),sourceRelative_angular_vel.Y(),sourceRelative_angular_vel.Z());
rbs.cov_angular_velocity = exported_link.cov_angular_velocity;
rbs.time = time;
exported_link.port->write(rbs);

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());
sourceRelative_linear_acc.X(),sourceRelative_linear_acc.Y(),sourceRelative_linear_acc.Z());
rba.angular_acceleration = base::Vector3d(
source_angular_acc.X(),source_angular_acc.Y(),source_angular_acc.Z());
sourceRelative_angular_acc.X(),sourceRelative_angular_acc.Y(),sourceRelative_angular_acc.Z());
rba.cov_angular_acceleration = exported_link.cov_angular_acceleration;
rba.time = time;
exported_link.rba_port->write(rba);

0 comments on commit aead319

Please sign in to comment.