void WorldRosItem::publishModelStates()
{
gazebo_msgs::ModelStates model_states;
Item* item = world->childItem();
while(item) {
BodyItem* body = boost::dynamic_pointer_cast<BodyItem>(item);
if (body) {
model_states.name.push_back(body->name());
Link* link = body->body()->rootLink();
Vector3 pos = link->translation();
Quat rot = Quat(link->rotation());
geometry_msgs::Pose pose;
pose.position.x = pos(0);
pose.position.y = pos(1);
pose.position.z = pos(2);
pose.orientation.w = rot.w();
pose.orientation.x = rot.x();
pose.orientation.y = rot.y();
pose.orientation.z = rot.z();
model_states.pose.push_back(pose);
/*
twist.linear.x = linear_vel.x;
twist.linear.y = linear_vel.y;
twist.linear.z = linear_vel.z;
twist.angular.x = angular_vel.x;
twist.angular.y = angular_vel.y;
twist.angular.z = angular_vel.z;
model_states.twist.push_back(twist);
*/
}
item = item->nextItem();
}
pub_model_states_.publish(model_states);
}
void LeggedBodyBarImpl::moveCM(BodyItem::PositionType position)
{
const ItemList<BodyItem>& targetBodyItems = bodyBar->targetBodyItems();
for(size_t i=0; i < targetBodyItems.size(); ++i){
BodyItem* bodyItem = targetBodyItems[i];
Vector3 c = bodyItem->centerOfMass();
boost::optional<Vector3> p = bodyItem->getParticularPosition(position);
if(p){
c[0] = (*p)[0];
c[1] = (*p)[1];
}
if(!bodyItem->doLegIkToMoveCm(c, true)){
MessageView::instance()->notify(
fmt::format(_("The center of mass of {} cannt be moved to the target position\n"),
bodyItem->name()));
}
}
}