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);
}
Beispiel #2
0
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()));
        }
    }
}