Can you modify the state(pose/velocity/acceleration) of a model from a world plugin
So I want to globally control and observe the interactions of all objects in my environment. The obvious way to do this seems to be through the world plugin. However setting parameters like linear velocity seems to have no effect on the simulation. I'm curious if gazebo is internally blocking modifications of models through the world plugin or if there is a particular setting that needs to be modified to allow this to occur. Thank you
So this here's a quick simplified snippet showing as to what I'm trying to do and how:
Let me know if there is more info you would like
void World_Plugin::Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf){
this->world = _parent;
this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&Quasistatic_World_Plugin::OnUpdate, this, _1));
}
//pusher vel is just some velocity
//pusher is a modelPtr I've extracted from the world
void World_Plugin::OnUpdate(const common::UpdateInfo & /*_info*/){
this->pusher->SetLinearVel(math::Vector3(pusher_vel[0],pusher_vel[1], pusher_vel[2]));
}
World File
<?xml version="1.0"?>
<sdf version="1.4">
<world name="blank plane">
<plugin name = "quasistatic_world" filename="libquasistatic_plugin.so"/>
<include>
<uri>model://ground_plane</uri>
</include>
</world>
</sdf>
The model is added in the world plugin and looks something like this (simple box for now):
<sdf version="1.4">
<model name="pusher_box">
<pose>0 0 0.5 0 0 0</pose>
<static>true</static>
<link name="link">
<collision name="box_collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
</link>
</model>
</sdf>
The syntax looks ok, assuming that
this- pusher
is a valid ModelPtr. Can you put a print statement in the OnUpdate function that printsthis- pusher- GetWorldLinearVel()
?right so when I tested this the velocity is zero. Is there some code in the background blocking me from setting the velocity of this model or there some reason the ModelPtr could be invalid. pusher is set by calling pusher = world->GetModel(0); I attempted to modify other properties like pose and the model state does not seem to update
Your problem is calling
GetModel(0)
, you should pass a string with the name of your model. Using an integer will just give you the first model that was initialized, which is probably the ground_plane, which is static and always has zero velocity.Alright I'll give that a shot thank you