C++ (Cpp) InteractiveRobot::robotModel Examples

Example #1

File: collision_contact_tutorial.cpp Project: TheDash/moveit_pr2

int main(int argc, char **argv)
{
  ros::init (argc, argv, "acorn_play");
  ros::NodeHandle nh;

  InteractiveRobot robot;

  // create a PlanningScene
  g_planning_scene = new planning_scene::PlanningScene(robot.robotModel());

  // Add the world geometry to the PlanningScene's collision detection world
  Eigen::Affine3d world_cube_pose;
  double world_cube_size;
  robot.getWorldGeometry(world_cube_pose, world_cube_size);
  g_world_cube_shape.reset(new shapes::Box(world_cube_size, world_cube_size, world_cube_size));
  g_planning_scene->getWorldNonConst()->addToObject("world_cube", g_world_cube_shape, world_cube_pose);

  // Create a marker array publisher for publishing contact points
  g_marker_array_publisher = new ros::Publisher(nh.advertise<visualization_msgs::MarkerArray>("interactive_robot_marray",100));

  robot.setUserCallback(userCallback);

  help();

  ros::spin();

  delete g_planning_scene;
  delete g_marker_array_publisher;;

  ros::shutdown();
  return 0;
}

Example #2

File: attached_body_tutorial.cpp Project: mpomarlan/moveit_puzzle_demo

int main(int argc, char **argv)
{
  ros::init (argc, argv, "acorn_play");
  ros::NodeHandle nh;

  InteractiveRobot robot;

  // create a PlanningScene
  g_planning_scene = new planning_scene::PlanningScene(robot.robotModel());

  // Add the world geometry to the PlanningScene's collision detection world 
  Eigen::Affine3d world_cube_pose;
  double world_cube_size;
  robot.getWorldGeometry(world_cube_pose, world_cube_size);
  g_world_cube_shape.reset(new shapes::Box(world_cube_size, world_cube_size, world_cube_size));
  g_planning_scene->getWorldNonConst()->addToObject("world_cube", g_world_cube_shape, world_cube_pose);
  
  // Create a marker array publisher for publishing contact points
  g_marker_array_publisher = new ros::Publisher(nh.advertise<visualization_msgs::MarkerArray>("interactive_robot_marray",100));







  // Attach a bar object to the right gripper
  robot_state::LinkState *link = robot.robotState()->getLinkState("r_gripper_palm_link");
  //robot_state::LinkState *link = robot.robotState()->getLinkState("r_gripper_r_finger_link");
  //robot_state::LinkState *link = robot.robotState()->getLinkState("r_wrist_roll_link");
  //

  std::vector<shapes::ShapeConstPtr> shapes;
  EigenSTL::vector_Affine3d poses;

  shapes::ShapePtr bar_shape;
  bar_shape.reset(new shapes::Cylinder(0.02,1.0));
  //bar_shape.reset(new shapes::Box(0.02,.02,1));
  
  shapes.push_back(bar_shape);
  poses.push_back(Eigen::Affine3d(Eigen::Translation3d(0.12,0,0)));

  const robot_model::JointModelGroup* r_gripper_group = robot.robotModel()->getJointModelGroup("right_gripper");
  const std::vector<std::string>& touch_links = r_gripper_group->getLinkModelNames();

  robot.robotState()->attachBody("bar",
                                 shapes,
                                 poses,
                                 touch_links,
                                 "r_gripper_palm_link");


  robot.setUserCallback(userCallback);

  help();

  ros::spin();

  delete g_planning_scene;
  delete g_marker_array_publisher;;

  ros::shutdown();  
  return 0;
}