Home | Tutorials | Wiki | Issues
Ask Your Question
0

Porting existing physics simulation for quadrotors, UAVs, AUVs, etc to Gazebo as a ModelPlugin

asked 2018-02-06 14:28:02 -0600

ivaughn gravatar image

updated 2018-02-06 16:46:22 -0600

This started out as a question for what coordinate frame the "set" functions use; it looks like that was the wrong question (they use world frame, if it helps).

Suppose I have an existing C++ physics simulation for something that really doesn't interact with the world, like a UAV, AUV, etc. Maybe that existing simulator has decades of history. Maybe we're comparing the performance of an existing dynamics simulation to a new gazebo-based one while re-using a bunch of sensor plugins. Or modelling the system in Gazebo will take a while and the robot ships next week.

Whatever the reason, we wrap our fancy physics plugin, and at the end we want to set the model's pose, velocities, and accelerations so that our various simulated instruments that simulate those things work.

However I try to set the model state, it doesn't read back the value I set. Consider the following code snippet trying to set a model's state:

model->ResetPhysicsStates();
model->SetWorldPose(gz_pose);
model->SetLinearVel(gz_lin_vel);
model->SetAngularVel(gz_ang_vel);
model->SetLinearAccel(gz_lin_acc);
model->SetAngularAccel(gz_ang_acc);

math::Vector3 gz_world_lin_vel = model->GetWorldLinearVel();

math::Pose vehPose = model->GetWorldPose();
math::Vector3 body_lin_vel = vehPose.rot.RotateVectorReverse(gz_world_lin_vel);
math::Vector3 rel_lin_vel = model->GetRelativeLinearVel();

ROS_INFO_STREAM("Sim: " <<state.velocity.linear.x <<" " <<-state.velocity.linear.y <<" " <<-state.velocity.linear.z
                    <<" Set : " <<gz_lin_vel.x <<" " <<gz_lin_vel.y <<" " <<gz_lin_vel.z
                    <<" Got : "<<gz_world_lin_vel.x <<" " <<gz_world_lin_vel.y <<" " <<gz_world_lin_vel.z
                    <<" Recv: "<<body_lin_vel.x <<" "<<body_lin_vel.y <<" "<<body_lin_vel.z
                    <<" Recv: "<<rel_lin_vel.x <<" "<<rel_lin_vel.y <<" "<<rel_lin_vel.z);

The sign flips on the Sim state are fine (yes, I know it looks like I missed one; airplanes and subs all use fwd/stbd/down, unlike gazebo). However, the gz_lin_vel that I set is NOT the same as the gz_world_lin_vel that is returned from GetWorldLinearVel. The relative and global poses work as expected, so that's good, but it definitely looks like there's some integration going on behind the scenes or something.

If you only set poses it seems to work fine, but we have a bunch of existing gazebo plugins to measure velocities, angular rates, and accelerations that it would be nice to use.

Thoughts? Anybody done anything similar? How?

--Ian

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-02-07 17:45:25 -0600

ivaughn gravatar image

It appears that SetLinearVel operates on the center of gravity of a link, and NOT the origin of the link itself. GetWorldLinearVel returns the world-frame linear velocity of the origin of the link and applies the lever-arm correction to account for rotation-induced linear motion, as expected (at least for ODE-- not sure about the other physics engines).

The solution is to either set the center of gravity to be the link origin or apply the inverse linear velocities to make the vehicle rotate about the desired point.

The meta-solution is to just read the source code. It's relatively clean, and much more insightful than the API docs.

edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2018-02-06 14:28:02 -0600

Seen: 18 times

Last updated: Feb 07