Ask Your Question
0

Drawing the PR2 gripper in Rviz

asked 2012-02-01 04:26:51 -0500

updated 2014-11-22 17:05:41 -0500

ngrennan gravatar image

I'm trying to draw an interactive marker that shows the PR2 gripper. I've found this C++ code in the interactive_marker_helpers package (object_manipulation stack), file interactive_marker_helpers.cpp.

visualization_msgs::InteractiveMarker makeGripperMarker( const char *name, const geometry_msgs::PoseStamped &stamped,
                                                         float scale, float angle, bool view_facing, std_msgs::ColorRGBA color, bool use_color )
{
  visualization_msgs::InteractiveMarker int_marker;
  int_marker.header =  stamped.header;
  int_marker.name = name;
  int_marker.scale = 1.0; //scale;
  int_marker.pose = stamped.pose;

//  add6DofControl(int_marker, false);

  visualization_msgs::InteractiveMarkerControl control;

  visualization_msgs::Marker mesh;
  mesh.mesh_use_embedded_materials = !use_color;
  mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
  mesh.scale.x = scale;
  mesh.scale.y = scale;
  mesh.scale.z = scale;
  mesh.color = color;

  tf::Transform T1, T2;
  tf::Transform T_proximal, T_distal;

  T1.setOrigin(tf::Vector3(0.07691, 0.01, 0));
  T1.setRotation(tf::Quaternion(tf::Vector3(0,0,1),  angle));
  T2.setOrigin(tf::Vector3(0.09137, 0.00495, 0));
  T2.setRotation(tf::Quaternion(tf::Vector3(0,0,1), -angle));
  T_proximal = T1;
  T_distal = T1 * T2;

  mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae";
  mesh.pose.orientation.w = 1;
  control.markers.push_back( mesh );
  mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae";
  mesh.pose = object_manipulator::msg::createPoseMsg(T_proximal);
  control.markers.push_back( mesh );
  mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae";
  mesh.pose = object_manipulator::msg::createPoseMsg(T_distal);
  control.markers.push_back( mesh );

  T1.setOrigin(tf::Vector3(0.07691, -0.01, 0));
  T1.setRotation(tf::Quaternion(tf::Vector3(1,0,0), M_PI)*tf::Quaternion(tf::Vector3(0,0,1),  angle));
  T2.setOrigin(tf::Vector3(0.09137, 0.00495, 0));
  T2.setRotation(tf::Quaternion(tf::Vector3(0,0,1), -angle));
  T_proximal = T1;
  T_distal = T1 * T2;

  mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae";
  mesh.pose = object_manipulator::msg::createPoseMsg(T_proximal);
  control.markers.push_back( mesh );
  mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae";
  mesh.pose = object_manipulator::msg::createPoseMsg(T_distal);
  control.markers.push_back( mesh );

  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
  int_marker.controls.push_back( control );

  return int_marker;
}

I am translating this code to Python, but tf lacks the Transform class. Is there any replacement for Transform class in Python? Does anybody have a piece of Python code that draws a gripper in Rviz?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2012-02-03 02:14:40 -0500

updated 2012-02-05 22:36:05 -0500

Here is the answer to my own question. For the tf transformations I used pyeuclid, which I've grabbed, adapted to my needs and re-shared https://github.com/lorenzoriano/pyeuclid">here. The final code is for you to grab in the Ulster repository https://github.com/uu-isrc-robotics/uu-isrc-robotics-pr2-pkgs/blob/master/pr2_trajectory_markers/src/trajectory_markers.py">here.

BTW, the result is a cool package to create and save trajectories of the PR2 grippers using the interactive markers :-)

[EDIT] How can I accept my own answer so that this question can be marked as solved?

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2012-02-01 04:26:51 -0500

Seen: 840 times

Last updated: Feb 05 '12