Add the option for the external force orientation to be reperesented in the link frame

CHANGELOG.md

@@ -6,8 +6,12 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 ## [Unreleased]
 
 ### Added
+- Add the option for the external force orientation to be reperesented in the link frame with the `externalwrench` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/463).
 - Add the possibility to create different types of joints with the `linkattacher` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/461).
 
+### Fixed
+- Fixed the case if the user writes any 1-word command other than single, the mode is switched to multiple wrenches mode (see https://github.com/robotology/gazebo-yarp-plugins/pull/418). Now, if the user writes any 1-word command other than `single`, `multiple`, `orient_global`, `orient_local`, they will receive an error message (https://github.com/robotology/gazebo-yarp-plugins/pull/463).
+
 ## [3.3.0] - 2019-12-13
 
 ### Added

plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh

@@ -29,6 +29,10 @@ public:
     // single wrench or multiple wrenches
     std::string         m_mode;
 
+    // variable for orientation mode
+    // global or local orientation modes
+    std::string         m_orient;
+
     int                 wrenchCount;
 
     virtual bool        threadInit();

plugins/externalwrench/include/gazebo/ExternalWrench.hh

@@ -80,7 +80,8 @@ public:
     void setTick(double& tickTime);
     void setTock(double& tockTime);
     void setVisual();
-    void applyWrench();
+    void applyGlobalOrientationWrench();
+    void applyLocalOrientationWrench();
     void deleteWrench();
     void setModel();
 };

plugins/externalwrench/src/ApplyExternalWrench.cc

@@ -84,7 +84,14 @@ void ApplyExternalWrench::onUpdate(const gazebo::common::UpdateInfo& _info)
         bool duration_check = m_rpcThread.wrenchesVector.at(i).duration_done;
         if(duration_check == false)
         {
-            m_rpcThread.wrenchesVector.at(i).applyWrench();
+            if(this->m_rpcThread.m_orient == "orient_global")
+            {
+                m_rpcThread.wrenchesVector.at(i).applyGlobalOrientationWrench();
+            }
+            else if(this->m_rpcThread.m_orient == "orient_local")
+            {
+                m_rpcThread.wrenchesVector.at(i).applyLocalOrientationWrench();
+            }
         }
     }
     this->m_lock.unlock();
@@ -107,6 +114,9 @@ void ApplyExternalWrench::onReset()
     // Change the operation mode to default option 'single'
     this->m_rpcThread.m_mode = "single";
 
+    // Change the orientation to default option 'orient_global'
+    this->m_rpcThread.m_orient = "orient_global";
+
     // Reset wrench count
     this->m_rpcThread.wrenchCount = 0;
 
@@ -129,6 +139,9 @@ bool RPCServerThread::threadInit()
     // Set the default operation mode
     this->m_mode = "single";
 
+    // Set the default operation mode
+    this->m_orient = "orient_global";
+
     // Set wrench count default value
     this->wrenchCount = 0;
 
@@ -142,16 +155,18 @@ void RPCServerThread::run()
         m_rpcPort.read ( command,true );
         if ( command.get ( 0 ).asString() == "help" ) {
             this->m_reply.addVocab ( yarp::os::Vocab::encode ( "many" ) );
-            this->m_reply.addString ( "The defaul operation mode is with single wrench" );
+            this->m_reply.addString ( "The default operation mode is with single wrench" );
             this->m_reply.addString ( "Insert [single] or [multiple] to change the operation mode" );
+            this->m_reply.addString ( "The default frame orientation is the one of the base/root frame ([orient_global])" );
+            this->m_reply.addString ( "Insert [orient_global] or [orient_local] to change the frame orientation mode" );
             this->m_reply.addString ( "Insert a command with the following format:" );
             this->m_reply.addString ( "[link] [force] [torque] [duration]" );
             this->m_reply.addString ( "e.g. chest 10 0 0 0 0 0 1");
             this->m_reply.addString ( "[link]:     (string) Link ID of the robot as specified in robot's SDF" );
-            this->m_reply.addString ( "[force]:    (double x, y, z) Force components in N w.r.t. world reference frame" );
-            this->m_reply.addString ( "[torque]:   (double x, y, z) Torque components in N.m w.r.t world reference frame" );
+            this->m_reply.addString ( "[force]:    (double x, y, z) Force components in N w.r.t. world reference frame (or link reference frame if [orient_local] is selected)" );
+            this->m_reply.addString ( "[torque]:   (double x, y, z) Torque components in N.m w.r.t world reference frame (or link reference frame if [orient_local] is selected)" );
             this->m_reply.addString ( "[duration]: (double) Duration of the applied force in seconds" );
-            this->m_reply.addString ( "Note: The reference frame is the base/root robot frame with x pointing backwards and z upwards.");
+            this->m_reply.addString ( "Note: If orientation is set to [orient_global], the reference frame is the base/root robot frame with x pointing backwards and z upwards.");
             this->m_rpcPort.reply ( this->m_reply );
         } else{
             if((command.size() == 8) && (command.get(0).isString() \
@@ -216,27 +231,85 @@ void RPCServerThread::run()
             }
             else if (command.size() == 1 && command.get(0).isString()) {
 
-                this->m_mode = command.get(0).asString();
+                if (command.get(0).asString() == "single") {
+
+                    this->m_mode = command.get(0).asString();
+
+                    this->m_message = command.get(0).asString() + " wrench operation mode set";
+
+                    // Reset wrench count
+                    wrenchCount = 0;
+
+                    // Delete the previous wrenches
+                    if (wrenchesVector.size() != 0) {
+                        this->m_message = this->m_message + " . Clearing previous wrenches.";
+                        for (int i = 0; i < wrenchesVector.size(); i++)
+                        {
+                            ExternalWrench wrench = wrenchesVector.at(i);
+                            wrench.deleteWrench();
+                        }
+                        wrenchesVector.clear();
+                    }
+                }
+                else if (command.get(0).asString() == "multiple") {
+
+                    this->m_mode = command.get(0).asString();
+
+                    this->m_message = command.get(0).asString() + " wrench operation mode set";
+
+                    // Reset wrench count
+                    wrenchCount = 0;
+
+                    // Delete the previous wrenches
+                    if (wrenchesVector.size() != 0) {
+                        this->m_message = this->m_message + " . Clearing previous wrenches.";
+                        for (int i = 0; i < wrenchesVector.size(); i++)
+                        {
+                            ExternalWrench wrench = wrenchesVector.at(i);
+                            wrench.deleteWrench();
+                        }
+                        wrenchesVector.clear();
+                    }
+                }
+                else if (command.get(0).asString() == "orient_global") {
+                    this->m_orient = command.get(0).asString();
+
+                    this->m_message = command.get(0).asString() + " wrench orientation option set";
 
-                this->m_message = command.get(0).asString() + " wrench operation mode set";
+                    // Delete the previous wrenches
+                    if (wrenchesVector.size() != 0) {
+                        this->m_message = this->m_message + " . Clearing previous wrenches.";
+                        for (int i = 0; i < wrenchesVector.size(); i++)
+                        {
+                            ExternalWrench wrench = wrenchesVector.at(i);
+                            wrench.deleteWrench();
+                        }
+                        wrenchesVector.clear();
+                    }
+                }
+                else if (command.get(0).asString() == "orient_local") {
+                    this->m_orient = command.get(0).asString();
 
-                // Reset wrench count
-                wrenchCount = 0;
+                    this->m_message = command.get(0).asString() + " wrench orientation option set";
 
-                // Delete the previous wrenches
-                if (wrenchesVector.size() != 0) {
-                    this->m_message = this->m_message + " . Clearing previous wrenches.";
-                    for (int i = 0; i < wrenchesVector.size(); i++)
-                    {
-                        ExternalWrench wrench = wrenchesVector.at(i);
-                        wrench.deleteWrench();
+                    // Delete the previous wrenches
+                    if (wrenchesVector.size() != 0) {
+                        this->m_message = this->m_message + " . Clearing previous wrenches.";
+                        for (int i = 0; i < wrenchesVector.size(); i++)
+                        {
+                            ExternalWrench wrench = wrenchesVector.at(i);
+                            wrench.deleteWrench();
+                        }
+                        wrenchesVector.clear();
                     }
-                    wrenchesVector.clear();
+                }
+                else {
+                    this->m_reply.clear();
+                    this->m_message = "ERROR: Incorrect command format. Insert [help] to know the correct command format";
                 }
 
                 this->m_reply.addString (m_message);
                 this->m_rpcPort.reply ( m_reply );
-
             }
             else {
                 this->m_reply.clear();

plugins/externalwrench/src/ExternalWrench.cc

@@ -130,7 +130,7 @@ bool ExternalWrench::setWrench(physics::ModelPtr& _model,yarp::os::Bottle& cmd)
     else return false;
 }
 
-void ExternalWrench::applyWrench()
+void ExternalWrench::applyGlobalOrientationWrench()
 {
     if((tock-tick) < wrench.duration)
     {
@@ -147,7 +147,12 @@ void ExternalWrench::applyWrench()
         ignition::math::Vector3d newY = newZ.Cross (newX);
         ignition::math::Matrix4d rotation = ignition::math::Matrix4d (newX[0],newY[0],newZ[0],0,newX[1],newY[1],newZ[1],0,newX[2],newY[2],newZ[2],0, 0, 0, 0, 1);
         ignition::math::Quaterniond forceOrientation = rotation.Rotation();
-        ignition::math::Pose3d linkCoGPose (linkCoGPos - rotation*ignition::math::Vector3d ( 0,0,.15 ), forceOrientation);
+
+        //For the cylindrical shapes used to visualize the applied forces
+        const ignition::math::Vector3d cylinderHalfLength = ignition::math::Vector3d ( 0,0,-0.15 );
+
+        //Construct the pose of the cylindrical shape putting the cylinder end at force location
+        ignition::math::Pose3d cylinderPose (linkCoGPos - rotation*cylinderHalfLength, forceOrientation);
 #else
         math::Vector3d force (wrench.force[0], wrench.force[1], wrench.force[2]);
         math::Vector3d torque (wrench.torque[0], wrench.torque[1], wrench.torque[2]);
@@ -157,17 +162,133 @@ void ExternalWrench::applyWrench()
 
         math::Vector3d linkCoGPos = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied
         math::Vector3d newZ = force.Normalize(); // normalized force. I want the z axis of the cylinder's reference frame to coincide with my force vector
-        math::Vector3d newX = newZ.Cross (ignition::math::Vector3d::UnitZ);
+        math::Vector3d newX = newZ.Cross (math::Vector3d::UnitZ);
         math::Vector3d newY = newZ.Cross (newX);
-        math::Matrix4d rotation = ignition::math::Matrix4d (newX[0],newY[0],newZ[0],0,newX[1],newY[1],newZ[1],0,newX[2],newY[2],newZ[2],0, 0, 0, 0, 1);
+        math::Matrix4d rotation = math::Matrix4d (newX[0],newY[0],newZ[0],0,newX[1],newY[1],newZ[1],0,newX[2],newY[2],newZ[2],0, 0, 0, 0, 1);
         math::Quaterniond forceOrientation = rotation.Rotation();
-        math::Pose3d linkCoGPose (linkCoGPos - rotation*ignition::math::Vector3d ( 0,0,.15 ), forceOrientation);
+
+        //For the cylindrical shapes used to visualize the applied forces
+        const math::Vector3d cylinderHalfLength = math::Vector3d ( 0,0,-0.15 );
+
+        //Construct the pose of the cylindrical shape putting the cylinder end at force location
+        math::Pose3d cylinderPose (linkCoGPos - rotation*cylinderHalfLength, forceOrientation);
+#endif
+
+#if GAZEBO_MAJOR_VERSION == 7
+        msgs::Set(visualMsg.mutable_pose(), cylinderPose.Ign());
+#else
+        msgs::Set(visualMsg.mutable_pose(), cylinderPose);
+#endif
+#if GAZEBO_MAJOR_VERSION >= 9
+        msgs::Set(visualMsg.mutable_material()->mutable_ambient(), ignition::math::Color(color[0],color[1],color[2],color[3]));
+#else
+        msgs::Set(visualMsg.mutable_material()->mutable_ambient(),common::Color(color[0],color[1],color[2],color[3]));
+#endif
+        visualMsg.set_visible(1);
+        visPub->Publish(visualMsg);
+    }
+    else
+    {
+        deleteWrench();
+    }
+}
+
+void ExternalWrench::applyLocalOrientationWrench()
+{
+    if((tock-tick) < wrench.duration)
+    {
+#if GAZEBO_MAJOR_VERSION >= 8
+        ignition::math::Vector3d force (wrench.force[0], wrench.force[1], wrench.force[2]);
+        ignition::math::Vector3d torque (wrench.torque[0], wrench.torque[1], wrench.torque[2]);
+
+        //Apply the wrench locally to the link
+        link->AddRelativeForce(force);
+        link->AddRelativeTorque(torque);
+
+        //The abbreviations below are used in the following section
+        //Frames:
+        //World frame = W
+        //Link frame = L
+        //Link CoG frame = Lg
+        //Applied Force frame = F
+
+        //Quantities:
+        //Transformation = T
+        //Position 3d = P
+        //Rotation Matrix = R
+
+        //Examples:
+        //W_T_L = the Transformation from the World frame to the Link frame
+        //Lg_R_F = the Rotation from the Link CoG frame to the Applied Force frame
+
+        //Transformation from Link CoG (Lg) frame -> Applied Force (F) frame
+        ignition::math::Vector3d newZ = force.Normalized(); // normalized force. I want the z axis of the cylinder's reference frame to coincide with my force vector
+        ignition::math::Vector3d newX = newZ.Cross (ignition::math::Vector3d::UnitZ);
+        ignition::math::Vector3d newY = newZ.Cross (newX);
+
+        ignition::math::Matrix4d Lg_T_F = ignition::math::Matrix4d (newX[0],newY[0],newZ[0],0,newX[1],newY[1],newZ[1],0,newX[2],newY[2],newZ[2],0, 0, 0, 0, 1);
+
+        //Transformation from World frame -> Link CoG (Lg) frame
+        ignition::math::Matrix4d W_T_Lg = ignition::math::Matrix4d (link->WorldCoGPose());
+
+        //Transformation from World frame ->  Applied Force (F) frame
+        ignition::math::Matrix4d W_T_F = W_T_Lg*Lg_T_F;
+        ignition::math::Quaterniond W_R_F = W_T_F.Rotation();
+
+        //For the cylindrical shapes used to visualize the applied forces
+        const ignition::math::Vector3d cylinderHalfLength = ignition::math::Vector3d ( 0,0,-0.15 );
+
+        //Construct the pose of the cylindrical shape putting the cylinder end at force location
+        ignition::math::Pose3d cylinderPose (W_T_F*cylinderHalfLength, W_R_F);
+#else
+        math::Vector3d force (wrench.force[0], wrench.force[1], wrench.force[2]);
+        math::Vector3d torque (wrench.torque[0], wrench.torque[1], wrench.torque[2]);
+
+        //Apply the wrench locally to the link
+        link->AddRelativeForce(force);
+        link->AddRelativeTorque(torque);
+
+        //The abbreviations below are used in the following section
+        //Frames:
+        //World frame = W
+        //Link frame = L
+        //Link CoG frame = Lg
+        //Applied Force frame = F
+
+        //Quantities:
+        //Transformation = T
+        //Position 3d = P
+        //Rotation Matrix = R
+
+        //Examples:
+        //W_T_L = the Transformation from the World frame to the Link frame
+        //Lg_R_F = the Rotation from the Link CoG frame to the Applied Force frame
+
+        //Transformation from Link CoG (Lg) frame -> Applied Force (F) frame
+        math::Vector3d newZ = force.Normalized(); // normalized force. I want the z axis of the cylinder's reference frame to coincide with my force vector
+        math::Vector3d newX = newZ.Cross (math::Vector3d::UnitZ);
+        math::Vector3d newY = newZ.Cross (newX);
+
+        math::Matrix4d Lg_T_F = math::Matrix4d (newX[0],newY[0],newZ[0],0,newX[1],newY[1],newZ[1],0,newX[2],newY[2],newZ[2],0, 0, 0, 0, 1);
+
+        //Transformation from World frame -> Link CoG (Lg) frame
+        math::Matrix4d W_T_Lg = math::Matrix4d (link->WorldCoGPose());
+
+        //Transformation from World frame ->  Applied Force (F) frame
+        math::Matrix4d W_T_F = W_T_Lg*Lg_T_F;
+        math::Quaterniond W_R_F = W_T_F.Rotation();
+
+        //For the cylindrical shapes used to visualize the applied forces
+        const math::Vector3d cylinderHalfLength = math::Vector3d ( 0,0,-0.15 );
+
+        //Construct the pose of the cylindrical shape putting the cylinder end at force location
+        math::Pose3d cylinderPose (W_T_F*cylinderHalfLength, W_R_F);
 #endif
 
 #if GAZEBO_MAJOR_VERSION == 7
-        msgs::Set(visualMsg.mutable_pose(), linkCoGPose.Ign());
+        msgs::Set(visualMsg.mutable_pose(), cylinderPose.Ign());
 #else
-        msgs::Set(visualMsg.mutable_pose(), linkCoGPose);
+        msgs::Set(visualMsg.mutable_pose(), cylinderPose);
 #endif
 #if GAZEBO_MAJOR_VERSION >= 9
         msgs::Set(visualMsg.mutable_material()->mutable_ambient(), ignition::math::Color(color[0],color[1],color[2],color[3]));