Added new fields to Body and Robot.

CMakeLists.txt

@@ -57,10 +57,13 @@ find_package(Qt4 COMPONENTS QtCore  REQUIRED)
    Robot.msg
    Grasp.msg
    Planner.msg
+   PlannerResult.msg
    SearchEnergy.msg
    SearchSpace.msg
    SearchContact.msg
    Energy.msg
+   PregraspParams.msg
+   Contact.msg
  )
 
 ## Generate services in the 'srv' folder

include/graspit_interface.h

@@ -12,6 +12,9 @@
 
 //Message includes
 #include <graspit_interface/SearchSpace.h>
+#include <graspit_interface/PlannerResult.h>
+#include <graspit_interface/PregraspParams.h>
+#include <graspit_interface/Contact.h>
 
 // Service includes
 #include <graspit_interface/Robot.h>
@@ -44,6 +47,7 @@
 #include <graspit_interface/FindInitialContact.h>
 #include <graspit_interface/DynamicAutoGraspComplete.h>
 
+
 // ActionServer includes
 #include <graspit_interface/PlanGraspsAction.h>
 
@@ -194,6 +198,28 @@ class GraspitInterface : public QObject, public Plugin
   bool dynamicAutoGraspCompleteCB(graspit_interface::DynamicAutoGraspComplete::Request &request,
                                   graspit_interface::DynamicAutoGraspComplete::Response &response);
 
+  // Convenience functions for converting between pose types:
+    inline geometry_msgs::Pose transfToRosMsg(transf pose) {
+        geometry_msgs::Pose ret;
+        ret.position.x = pose.translation().x() / 1000.0;
+        ret.position.y = pose.translation().y() / 1000.0;;
+        ret.position.z = pose.translation().z() / 1000.0;;
+        ret.orientation.w = pose.rotation().w;
+        ret.orientation.x = pose.rotation().x;
+        ret.orientation.y = pose.rotation().y;
+        ret.orientation.z = pose.rotation().z;
+        return ret;
+    }
+    inline transf rosMsgToTransf(geometry_msgs::Pose pose) {
+        Quaternion q(pose.orientation.w, pose.orientation.x, 
+                pose.orientation.y, pose.orientation.z);
+        vec3 p(pose.position.x * 1000.0, pose.position.y * 1000.0, 
+                pose.position.z * 1000.0);
+        transf ret(q, p);
+        return ret;
+    }
+
+
   //ActionServer callbacks
   void PlanGraspsCB(const graspit_interface::PlanGraspsGoalConstPtr &goal);
 

msg/Body.msg

@@ -1,3 +1,19 @@
 std_msgs/Header header
 
-geometry_msgs/Pose pose
+string  element_name #UID for the body's world element.
+geometry_msgs/Pose pose
+
+# Other properties
+int32     material
+float64  transparency
+float64  youngs_modulus
+bool    is_dynamic
+bool is_elastic
+
+# Dynamic body properties. -1/None if body is not dynamic:
+float64 mass
+geometry_msgs/Inertia inertia
+#this is DynamicBody()->getCoG and DynamicBody->getInertia()
+geometry_msgs/Accel accel
+geometry_msgs/Twist velocity
+float64 max_radius

msg/Contact.msg

@@ -0,0 +1,8 @@
+string body1
+string body2
+
+geometry_msgs/Point position # in body1 frame.
+
+float64 cof # coefficient of friction.
+
+

msg/GraspableBody.msg

@@ -1,3 +1,4 @@
 std_msgs/Header header
 
-geometry_msgs/Pose pose
+string element_name #UID for the body's world element.
+geometry_msgs/Pose pose

msg/PregraspParams.msg

@@ -0,0 +1,3 @@
+float64[] open_dofs_by # How much to open/close hand for pregrasp.
+float64 retreat_by # Dist (in meters) to move hand backwards along approach vector.
+

msg/Robot.msg

@@ -1,5 +1,10 @@
+# Constants:
 std_msgs/Header header
+string element_name #UID for the body's world element.
+geometry_msgs/Pose approach_direction
 
+# Variables:
 geometry_msgs/Pose pose
 sensor_msgs/JointState[] joints
 float64[] dofs
+Contact[] contacts