$search

set_model_pose.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/



#include <ros/ros.h>
#include <urdf/model.h>

#include <tinyxml.h>

#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"

#include <std_srvs/Empty.h>
#include <gazebo_msgs/GetModelProperties.h>
#include <gazebo_msgs/GetModelState.h>
#include <gazebo_msgs/GetLinkState.h>
#include <gazebo_msgs/SetLinkState.h>
#include <gazebo_msgs/LinkState.h>
#include <geometry_msgs/Pose.h>
#include <gazebo/World.hh>
#include <gazebo/Model.hh>
#include <gazebo/Entity.hh>
#include <gazebo/Body.hh>

#include <boost/shared_ptr.hpp>

void setup_transform(btTransform &transform, urdf::Pose pose)
{
    btMatrix3x3 mat;
    mat.setRotation(btQuaternion(pose.rotation.x,pose.rotation.y,pose.rotation.z,pose.rotation.w));
    transform = btTransform(mat,btVector3(pose.position.x,pose.position.y,pose.position.z));
}
void setup_transform(btTransform &transform, geometry_msgs::Pose pose)
{
    btMatrix3x3 mat;
    mat.setRotation(btQuaternion(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w));
    transform = btTransform(mat,btVector3(pose.position.x,pose.position.y,pose.position.z));
}


void traverse_tree(boost::shared_ptr<const urdf::Link> link, const btTransform &transform, ros::ServiceClient set_link_state_client, std::string gazebo_model_name, int level = 0)
{
  // keep track of local transform for recursion
  btTransform currentTransform = transform;

  /* compute global transform */
  btTransform localTransform;
  // this is the transform from parent link to current link
  // this transform does not exist for the root link
  if (link->parent_joint)
  {
    ROS_DEBUG("%s has a parent joint",link->name.c_str());
    setup_transform(localTransform, link->parent_joint->parent_to_joint_origin_transform);
    currentTransform *= localTransform;

    // get position
    btVector3 pz = currentTransform.getOrigin();
    //double cpos[3] = { pz.x(), pz.y(), pz.z() };
    // get rotation
    btMatrix3x3 mat = currentTransform.getBasis();
    // mat.getRPY is broken, get back quaternion, use urdf::Rotation to get from quaternion to rpy
    btQuaternion btq;
    mat.getRotation(btq);
    //urdf::Rotation q(btq.x(),btq.y(),btq.z(),btq.w());

    // massless bodies are not simulated
    if (link->inertial && link->inertial->mass != 0)
    {
      if (link->parent_joint->type == urdf::Joint::PRISMATIC)
      {
        double joint_position = 0.5;
        btVector3 axis( link->parent_joint->axis.x,link->parent_joint->axis.y,link->parent_joint->axis.z );
        pz += joint_position * axis;

      }
      else if (link->parent_joint->type == urdf::Joint::REVOLUTE ||
               link->parent_joint->type == urdf::Joint::CONTINUOUS)
      {
        // rotate joint by some angle about joint axis
        btVector3 axis( link->parent_joint->axis.x,link->parent_joint->axis.y,link->parent_joint->axis.z );
        double test_joint_angle = -0.5;
        btQuaternion joint_rot(axis,test_joint_angle); // TEST!  try .1 radians for all joints
        // update currentTransform by additional joint rotation
        btq *= joint_rot;
        mat.setRotation(btq);
      }

      // update currentTransform
      currentTransform.setOrigin(pz);
      currentTransform.setBasis(mat);

      // display result
      ROS_DEBUG("%s has currentTransform: [%f %f %f, %f %f %f %f]",link->name.c_str(),pz.x(), pz.y(), pz.z(),btq.x(),btq.y(),btq.z(),btq.w());

      // just for testing, go through and set each link state
      gazebo_msgs::SetLinkState set_link_state;
      set_link_state.request.link_state.link_name = gazebo_model_name + std::string("::") + link->name;
      set_link_state.request.link_state.reference_frame = "world";
      set_link_state.request.link_state.pose.position.x = pz.x();
      set_link_state.request.link_state.pose.position.y = pz.y();
      set_link_state.request.link_state.pose.position.z = pz.z();
      set_link_state.request.link_state.pose.orientation.x = btq.x();
      set_link_state.request.link_state.pose.orientation.y = btq.y();
      set_link_state.request.link_state.pose.orientation.z = btq.z();
      set_link_state.request.link_state.pose.orientation.w = btq.w();
      if (!set_link_state_client.call(set_link_state))
        ROS_ERROR("unable to sete link [%s] state",set_link_state.request.link_state.link_name.c_str());
    }

  }
  else
    ROS_DEBUG("%s has no parent joint, is a root link?",link->name.c_str());

  level+=2;
  for (std::vector<boost::shared_ptr<urdf::Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
  {
    if (*child)
    {
      // first grandchild
      traverse_tree(*child,currentTransform,set_link_state_client,gazebo_model_name,level);
    }
    else
      ROS_ERROR("link: %s has a null child",link->name.c_str());
  }

}


int main(int argc, char **argv)
{
  // start a gazebo ros node
  ros::init(argc,argv,"gazebo_model_pose_setter");
  ros::NodeHandle nh("");

  std::string gazebo_model_name = "pr2";  //FIXME: Hardcoded model name
  std::string urdf_param_name = "robot_description"; //FIXME: Hardcoded param name

  // we need the urdf to do the transforms
  // search and wait for robot_description on param server
  urdf::Model urdf_model;
  if (!urdf_model.initParam(urdf_param_name))
    ROS_ERROR("unable to get urdf xml from param %s",urdf_param_name.c_str());
  // get gazebo model from urdf model name
  ROS_INFO("urdf model name [%s]",urdf_model.getName().c_str());


  // this will work only in a plugin
  //gazebo::Model* gazebo_model = dynamic_cast<gazebo::Model*>(gazebo::World::Instance()->GetEntityByName(urdf_model.getName()));
  //if (gazebo_model == NULL)
  //  ROS_ERROR("unable to get gazebo model with name [%s]",urdf_model.getName().c_str());
  // check to make sure that root_link is the canonicalBody of the Gazebo::Model
  //if (root_link->name != gazebo_model->GetCanonicalBody()->GetName())
  //  ROS_ERROR("gazebo_model canonical body is not the root link!");


  // get root link
  boost::shared_ptr<const urdf::Link> root_link = urdf_model.getRoot();
  if (!root_link)
    ROS_ERROR("urdf has no root");

  // make the service call to pause gazebo
  ros::ServiceClient pause_client = nh.serviceClient<std_srvs::Empty>("gazebo/pause_physics");
  pause_client.waitForExistence();
  std_srvs::Empty empty_pause;
  if (!pause_client.call(empty_pause))
    ROS_ERROR("unable to pause physics engine");

  // make the service call to get model properties for list of joints
  ros::ServiceClient model_properties_client = nh.serviceClient<gazebo_msgs::GetModelProperties>("gazebo/get_model_properties");
  gazebo_msgs::GetModelProperties model_properties;
  model_properties.request.model_name = urdf_model.getName();
  if (!model_properties_client.call(model_properties))
    ROS_ERROR("unable to get model properties");

  // make the service call to get pr2 model pose
  ros::ServiceClient model_state_client = nh.serviceClient<gazebo_msgs::GetModelState>("gazebo/get_model_state");
  model_state_client.waitForExistence();
  gazebo_msgs::GetModelState model_state;
  model_state.request.model_name = gazebo_model_name;
  model_state.request.relative_entity_name = "world";
  if (!model_state_client.call(model_state))
    ROS_ERROR("unable to get model state");

  // make the service call to get root link pose
  ros::ServiceClient root_link_state_client = nh.serviceClient<gazebo_msgs::GetLinkState>("gazebo/get_link_state");
  root_link_state_client.waitForExistence();
  gazebo_msgs::GetLinkState root_link_state;
  //root_link_state.request.link_name = gazebo_model->GetCanonicalBody()->GetScopedName(); // if we were in a plugin
  root_link_state.request.link_name = gazebo_model_name + std::string("::") + root_link->name; // scope name for gazebo
  root_link_state.request.reference_frame = "world";
  if (!root_link_state_client.call(root_link_state))
    ROS_ERROR("unable to get root link state");

  // setup transform, using root link pose
  btTransform transform;
  setup_transform(transform,root_link_state.response.link_state.pose);

  // testing
  ros::ServiceClient set_link_state_client = nh.serviceClient<gazebo_msgs::SetLinkState>("gazebo/set_link_state");
  set_link_state_client.waitForExistence();

  // traverse through urdf and get link poses based on new joint angles
  traverse_tree(root_link, transform, set_link_state_client, gazebo_model_name);

  // set individual body poses from the new transforms
  

  // make the service call to unpause gazebo
  ros::ServiceClient unpause_client = nh.serviceClient<std_srvs::Empty>("gazebo/unpause_physics");
  unpause_client.waitForExistence();
  std_srvs::Empty empty_unpause;
  if (!unpause_client.call(empty_unpause))
    ROS_ERROR("unable to unpause physics engine");

  return 0;
}

---

gazebo
Author(s): Nate Koenig, Andrew Howard, with contributions from many others. See web page for a full credits llist.
autogenerated on Sat Mar 2 13:39:02 2013