Updated ConstraintActuator implementaiton to allow softbody grasping …

…(Issue #179) (#191) * Added implementation for getting Actuator and Sensor from ObjectManager * Added identifiers (from ADF specification) to all afObject ROS msgs * Constraint Actuators can now be actuated by giving a sensor's "identifier" * Updated example script for grasping objects from sensor data * Added softbodies and updated the launch file to demo grasping softbodies * Updated readme for softbody grasping * Cleaup
WPI-AIM · Oct 24, 2022 · 0e2aa6f · 0e2aa6f
1 parent a52f874
commit 0e2aa6f
Show file tree

Hide file tree

Showing 21 changed files with 178 additions and 43 deletions.
diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp
@@ -5152,11 +5152,39 @@ afRigidBodyPtr afObjectManager::getRootRigidBody(afRigidBodyPtr a_bodyPtr){
     return rootParentBody;
 }
 
+///
+/// \brief afObjectManager::getJoint
+/// \param a_name
+/// \param suppress_warning
+/// \return
+///
 afJointPtr afObjectManager::getJoint(string a_name, bool suppress_warning){
     return (afJointPtr)getBaseObject(a_name, getJointMap(), suppress_warning);
 }
 
 
+///
+/// \brief afObjectManager::getActuator
+/// \param a_name
+/// \param suppress_warning
+/// \return
+///
+afActuatorPtr afObjectManager::getActuator(string a_name, bool suppress_warning){
+    return (afActuatorPtr)getBaseObject(a_name, getActuatorMap(), suppress_warning);
+}
+
+///
+/// \brief afObjectManager::getSensor
+/// \param a_name
+/// \param suppress_warning
+/// \return
+///
+afSensorPtr afObjectManager::getSensor(string a_name, bool suppress_warning){
+    return (afSensorPtr)getBaseObject(a_name, getSensorMap(), suppress_warning);
+
+}
+
+
 ///
 /// \brief afObjectManager::getSoftBody
 /// \param a_name

diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h
@@ -783,9 +783,9 @@ class afObjectManager{
 
     afJointPtr getJoint(string a_name, bool suppress_warning=false);
 
-    afActuatorPtr getActuator(string a_name);
+    afActuatorPtr getActuator(string a_name, bool suppress_warning=false);
 
-    afSensorPtr getSensor(string a_name);
+    afSensorPtr getSensor(string a_name, bool suppress_warning=false);
 
     afVehiclePtr getVehicle(string a_name, bool suppress_warning=false);
 

diff --git a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp
@@ -75,6 +75,7 @@ int afObjectCommunicationPlugin::init(const afBaseObjectPtr a_afObjectPtr, const
 
     string objName = m_objectPtr->getName() + m_objectPtr->getGlobalRemapIdx();
     string objNamespace = m_objectPtr->getNamespace();
+    string objQualifiedIdentifier = m_objectPtr->getQualifiedIdentifier() + m_objectPtr->getGlobalRemapIdx();
     int minFreq = m_objectPtr->getMinPublishFrequency();
     int maxFreq = m_objectPtr->getMaxPublishFrequency();
     double timeOut = 0.5;
@@ -85,6 +86,7 @@ int afObjectCommunicationPlugin::init(const afBaseObjectPtr a_afObjectPtr, const
     case afType::ACTUATOR:
     {
         m_actuatorCommPtr.reset(new ambf_comm::Actuator(objName, objNamespace, minFreq, maxFreq, timeOut));
+        m_actuatorCommPtr->set_identifier(objQualifiedIdentifier);
         afActuatorPtr actPtr = (afActuatorPtr)m_objectPtr;
         switch (actPtr->m_actuatorType) {
         case afActuatorType::CONSTRAINT:
@@ -100,30 +102,35 @@ int afObjectCommunicationPlugin::init(const afBaseObjectPtr a_afObjectPtr, const
     case afType::CAMERA:
     {
         m_cameraCommPtr.reset(new ambf_comm::Camera(objName, objNamespace, minFreq, maxFreq, timeOut));
+        m_cameraCommPtr->set_identifier(objQualifiedIdentifier);
         success = true;
     }
         break;
     case afType::LIGHT:
     {
         m_lightCommPtr.reset(new ambf_comm::Light(objName, objNamespace, minFreq, maxFreq, timeOut));
+        m_lightCommPtr->set_identifier(objQualifiedIdentifier);
         success = true;
     }
         break;
     case afType::OBJECT:
     {
         m_objectCommPtr.reset(new ambf_comm::Object(objName, objNamespace, minFreq, maxFreq, timeOut));
+        m_objectCommPtr->set_identifier(objQualifiedIdentifier);
         success = true;
     }
         break;
     case afType::RIGID_BODY:
     {
         m_rigidBodyCommPtr.reset(new ambf_comm::RigidBody(objName, objNamespace, minFreq, maxFreq, timeOut));
+        m_rigidBodyCommPtr->set_identifier(objQualifiedIdentifier);
         success = true;
     }
         break;
     case afType::SENSOR:
     {
         m_sensorCommPtr.reset(new ambf_comm::Sensor(objName, objNamespace, minFreq, maxFreq, timeOut));
+        m_sensorCommPtr->set_identifier(objQualifiedIdentifier);
         afSensorPtr senPtr = (afSensorPtr) m_objectPtr;
         switch (senPtr->m_sensorType) {
         case afSensorType::RAYTRACER:
@@ -141,6 +148,7 @@ int afObjectCommunicationPlugin::init(const afBaseObjectPtr a_afObjectPtr, const
     case afType::VEHICLE:
     {
         m_vehicleCommPtr.reset(new ambf_comm::Vehicle(objName, objNamespace, minFreq, maxFreq, timeOut));
+        m_actuatorCommPtr->set_identifier(objQualifiedIdentifier);
         success = true;
     }
         break;
@@ -300,22 +308,35 @@ void afObjectCommunicationPlugin::actuatorFetchCommand(afActuatorPtr actPtr, dou
                 // Constraint is active. Ignore request
                 return;
             }
-            string body_name = cmd.body_name.data;
-            if (cmd.use_offset){
-                // Offset of constraint (joint) in sensed body (child)
-                btTransform T_jINc;
-                T_jINc.setOrigin(btVector3(cmd.body_offset.position.x,
-                                           cmd.body_offset.position.y,
-                                           cmd.body_offset.position.z));
-
-                T_jINc.setRotation(btQuaternion(cmd.body_offset.orientation.x,
-                                                cmd.body_offset.orientation.y,
-                                                cmd.body_offset.orientation.z,
-                                                cmd.body_offset.orientation.w));
-                castPtr->actuate(body_name, T_jINc);
+            else if (cmd.use_sensor_data){
+                std::string sensorName = cmd.sensor_identifier.data;
+                afSensorPtr senPtr = actPtr->m_afWorld->getSensor(sensorName);
+                if (senPtr){
+                    castPtr->actuate(senPtr);
+                }
+                else{
+                    cerr << "ERROR! IN ACTUATOR CALLBACK " << castPtr->getName() <<
+                            ", REQUESTED SENSOR NAME " << sensorName << " NOT FOUND. IGNORING!" << endl;
+                }
             }
             else{
-                castPtr->actuate(body_name);
+                string body_name = cmd.body_name.data;
+                if (cmd.use_offset){
+                    // Offset of constraint (joint) in sensed body (child)
+                    btTransform T_jINc;
+                    T_jINc.setOrigin(btVector3(cmd.body_offset.position.x,
+                                               cmd.body_offset.position.y,
+                                               cmd.body_offset.position.z));
+
+                    T_jINc.setRotation(btQuaternion(cmd.body_offset.orientation.x,
+                                                    cmd.body_offset.orientation.y,
+                                                    cmd.body_offset.orientation.z,
+                                                    cmd.body_offset.orientation.w));
+                    castPtr->actuate(body_name, T_jINc);
+                }
+                else{
+                    castPtr->actuate(body_name);
+                }
             }
         }
         else{
@@ -333,7 +354,6 @@ void afObjectCommunicationPlugin::actuatorUpdateState(afActuatorPtr actPtr, doub
 {
     m_actuatorCommPtr->m_writeMtx.lock();
     setTimeStamps(m_objectPtr->m_afWorld->getWallTime(), m_objectPtr->m_afWorld->getSimulationTime(), m_objectPtr->getCurrentTimeStamp());
-    m_actuatorCommPtr->set_name(actPtr->getName());
     m_actuatorCommPtr->set_parent_name(actPtr->m_parentName);
     m_actuatorCommPtr->m_writeMtx.unlock();
     m_actuatorCommPtr->enableComm();
@@ -809,7 +829,6 @@ void afObjectCommunicationPlugin::sensorUpdateState(afSensorPtr senPtr, double d
     {
         afRayTracerSensorPtr raySenPtr = (afRayTracerSensorPtr) senPtr;
         m_sensorCommPtr->set_count(raySenPtr->getCount());
-        m_sensorCommPtr->set_name(raySenPtr->getName());
         m_sensorCommPtr->set_parent_name(raySenPtr->m_parentName);
         m_sensorCommPtr->set_range(raySenPtr->m_range);
         cVector3d pos = raySenPtr->getLocalPos();

diff --git a/ambf_ros_modules/ambf_client/python/ambf_actuator.py b/ambf_ros_modules/ambf_client/python/ambf_actuator.py
@@ -74,8 +74,19 @@ def actuate(self, obj_name, pose=None):
         self._cmd.body_name.data = obj_name
         self._cmd.actuate = True
 
+    def actuate_from_sensor_data(self, sensor_identifer):
+        """
+        :param sensor_name:
+        :return:
+        """
+        self._cmd.use_sensor_data = True
+        self._cmd.sensor_identifier.data = sensor_identifer
+        self._cmd.actuate = True
+
     def deactuate(self):
         """
         :return:
         """
         self._cmd.actuate = False
+        self._cmd.use_sensor_data = False
+        self._cmd.sensor_identifier.data = ''
diff --git a/ambf_ros_modules/ambf_client/python/ambf_base_object.py b/ambf_ros_modules/ambf_client/python/ambf_base_object.py
@@ -172,6 +172,12 @@ def get_rot_command(self):
         else:
             return self._state.pose.orientation
 
+    def get_identifier(self):
+        """
+        :return:
+        """
+        return self._state.identifier.data
+
     def get_name(self):
         """
         Get the name of this object

diff --git a/ambf_ros_modules/ambf_msgs/msg/ActuatorCmd.msg b/ambf_ros_modules/ambf_msgs/msg/ActuatorCmd.msg
@@ -3,3 +3,5 @@ bool actuate
 std_msgs/String body_name
 bool use_offset
 geometry_msgs/Pose body_offset
+bool use_sensor_data
+std_msgs/String sensor_identifier
diff --git a/ambf_ros_modules/ambf_msgs/msg/ActuatorState.msg b/ambf_ros_modules/ambf_msgs/msg/ActuatorState.msg
@@ -2,6 +2,7 @@ Header header
 uint32 sim_step
 std_msgs/String type
 std_msgs/String parent_name
+std_msgs/String identifier
 std_msgs/String name
 float32 wall_time
 float32 sim_time

diff --git a/ambf_ros_modules/ambf_msgs/msg/CameraState.msg b/ambf_ros_modules/ambf_msgs/msg/CameraState.msg
@@ -1,6 +1,7 @@
 Header header
 uint32 sim_step
 std_msgs/String parent_name
+std_msgs/String identifier
 std_msgs/String name
 float32 wall_time
 float32 sim_time

diff --git a/ambf_ros_modules/ambf_msgs/msg/LightState.msg b/ambf_ros_modules/ambf_msgs/msg/LightState.msg
@@ -1,6 +1,7 @@
 Header header
 uint32 sim_step
 std_msgs/String parent_name
+std_msgs/String identifier
 std_msgs/String name
 float32 wall_time
 float32 sim_time

diff --git a/ambf_ros_modules/ambf_msgs/msg/ObjectState.msg b/ambf_ros_modules/ambf_msgs/msg/ObjectState.msg
@@ -1,5 +1,6 @@
 Header header
 uint32 sim_step
+std_msgs/String identifier
 std_msgs/String name
 float32 wall_time
 float32 sim_time

diff --git a/ambf_ros_modules/ambf_msgs/msg/RigidBodyState.msg b/ambf_ros_modules/ambf_msgs/msg/RigidBodyState.msg
@@ -1,5 +1,6 @@
 Header header
 uint32 sim_step
+std_msgs/String identifier
 std_msgs/String name
 float32 wall_time
 float32 sim_time

diff --git a/ambf_ros_modules/ambf_msgs/msg/SensorState.msg b/ambf_ros_modules/ambf_msgs/msg/SensorState.msg
@@ -2,6 +2,7 @@ Header header
 uint32 sim_step
 std_msgs/String type
 std_msgs/String parent_name
+std_msgs/String identifier
 std_msgs/String name
 float32 wall_time
 float32 sim_time

diff --git a/ambf_ros_modules/ambf_msgs/msg/SoftBodyState.msg b/ambf_ros_modules/ambf_msgs/msg/SoftBodyState.msg
@@ -1,5 +1,6 @@
 Header header
 uint32 sim_step
+std_msgs/String identifier
 std_msgs/String name
 float32 wall_time
 float32 sim_time

diff --git a/ambf_ros_modules/ambf_msgs/msg/VehicleState.msg b/ambf_ros_modules/ambf_msgs/msg/VehicleState.msg
@@ -1,5 +1,6 @@
 Header header
 uint32 sim_step
+std_msgs/String identifier
 std_msgs/String name
 float32 wall_time
 float32 sim_time

diff --git a/ambf_ros_modules/ambf_server/include/ambf_server/RosComBase.h b/ambf_ros_modules/ambf_server/include/ambf_server/RosComBase.h
@@ -142,6 +142,10 @@ class RosComBase{
         m_State.name.data = name;
     }
 
+    inline void set_identifier(std::string identifier){
+        m_State.identifier.data = identifier;
+    }
+
     inline void set_time_stamp(double a_sec){
         m_State.header.stamp.fromSec(a_sec);
     }

diff --git a/ambf_ros_modules/examples/sensors_actuators_example/Images/sb_grasp1.png b/ambf_ros_modules/examples/sensors_actuators_example/Images/sb_grasp1.png
diff --git a/ambf_ros_modules/examples/sensors_actuators_example/Images/sb_grasp2.png b/ambf_ros_modules/examples/sensors_actuators_example/Images/sb_grasp2.png
diff --git a/ambf_ros_modules/examples/sensors_actuators_example/Images/sb_grasp3.png b/ambf_ros_modules/examples/sensors_actuators_example/Images/sb_grasp3.png
diff --git a/ambf_ros_modules/examples/sensors_actuators_example/README.md b/ambf_ros_modules/examples/sensors_actuators_example/README.md
@@ -52,3 +52,32 @@ This is an example of what you should see:
 
 ### Note:
 If you accidentally drop or misplace the box, you can click the simulation window and hit `CRTL + R` to reset the object poses in the simulation.
+
+### SOFT-BODY GRASPING (Beta Feature):
+
+Run the simulation as follows:
+
+``` bash
+cd <ambf>/bin/lin-x86_64/
+./ambf_simulator --launch_file <ambf>/ambf_ros_modules/examples/sensors_actuators_example/launch.yaml -l 0,1,2,3,4
+```
+The last two ADF files represent a textured ground and a softbody box.
+
+Similar to before, run the GUI with joint sliders and the sensing_and_grasping.py script.
+
+```
+cd <ambf>/ambf_ros_modules/examples/sensors_actuators_example/
+python sensing_and_grasping.py
+```
+
+By moving the KUKA robot such that the visible sensors touch the softbody, one can press the grasp button (from sensing_and_grasping GUI) to grasp the softbody. Pressing the release button will release the softbody.
+
+This is what the demo should look like.
+
+<img src="Images/sb_grasp1.png" title="Pre Grasp" width="30%" >
+
+<img src="Images/sb_grasp2.png" title="Grasp" width="30%">
+
+<img src="Images/sb_grasp3.png" title="Grasp and lift" width="30%">
+
+Checkout the `sensing_and_grasping.py` script for understanding how to use the `constraint actuators` for grasping a softbody using the new beta API.
diff --git a/ambf_ros_modules/examples/sensors_actuators_example/launch.yaml b/ambf_ros_modules/examples/sensors_actuators_example/launch.yaml
@@ -7,3 +7,5 @@ multibody configs:
   - "../../../ambf_models/descriptions/multi-bodies/robots/blender-kuka-lbr-med.yaml" #0
   - "./sensors_actuators.yaml" #1
   - "./box.yaml" #2
+  - "../../../ambf_models/descriptions/multi-bodies/environments/TexturedGround/env.yaml" #3
+  - "../../../ambf_models/descriptions/multi-bodies/sb/magic_box.yaml" #4