From b2fad6c5516c23c3522c6cf52b221f357b51fc1f Mon Sep 17 00:00:00 2001
From: Hosameldin <hosameldin.mohamed@iit.it>
Date: Mon, 27 Jan 2020 12:14:13 +0100
Subject: [PATCH 1/7] Add the option of the frame to represent the applied
 wrench

---
 .../include/gazebo/ApplyExternalWrench.hh     |  4 +
 .../externalwrench/src/ApplyExternalWrench.cc | 80 +++++++++++++++----
 2 files changed, 69 insertions(+), 15 deletions(-)

diff --git a/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh b/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh
index 584d8d056..0b7478b23 100644
--- a/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh
+++ b/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh
@@ -29,6 +29,10 @@ public:
     // single wrench or multiple wrenches
     std::string         m_mode;
 
+    // variable for orientation mode
+    // base link or local orientations
+    std::string         m_orient;
+
     int                 wrenchCount;
 
     virtual bool        threadInit();
diff --git a/plugins/externalwrench/src/ApplyExternalWrench.cc b/plugins/externalwrench/src/ApplyExternalWrench.cc
index 7919995cf..bbae72997 100644
--- a/plugins/externalwrench/src/ApplyExternalWrench.cc
+++ b/plugins/externalwrench/src/ApplyExternalWrench.cc
@@ -107,6 +107,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_base'
+    this->m_rpcThread.m_mode = "orient_base";
+
     // Reset wrench count
     this->m_rpcThread.wrenchCount = 0;
 
@@ -129,6 +132,9 @@ bool RPCServerThread::threadInit()
     // Set the default operation mode
     this->m_mode = "single";
 
+    // Set the default operation mode
+    this->m_orient = "orient_base";
+
     // Set wrench count default value
     this->wrenchCount = 0;
 
@@ -142,8 +148,10 @@ 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" );
+            this->m_reply.addString ( "Insert [orient_base] or [orient_local] to change the operation 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");
@@ -151,7 +159,7 @@ void RPCServerThread::run()
             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 ( "[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_base], 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 +224,69 @@ 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";
+                    this->m_message = command.get(0).asString() + " wrench operation mode set";
 
-                // Reset wrench count
-                wrenchCount = 0;
+                    // 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();
+                    // 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 if (command.get(0).asString() == "orient_base") {
+                    this->m_orient = command.get(0).asString();
+
+                    this->m_message = command.get(0).asString() + " wrench orientation option set";
+
+                    // Not clearing previous wrenches!
+
+                }
+                else if (command.get(0).asString() == "orient_local") {
+                    this->m_orient = command.get(0).asString();
+
+                    this->m_message = command.get(0).asString() + " wrench orientation option set";
+
+                    // Not clearing previous wrenches!
+
+                }
+                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();

From b674a933bc6d5950fa603269c475da25e7b60eb7 Mon Sep 17 00:00:00 2001
From: Hosameldin <hosameldin.mohamed@iit.it>
Date: Tue, 28 Jan 2020 01:34:01 +0100
Subject: [PATCH 2/7] Add the functions `applyGlobalOrientationWrench()` and
 `applyLocalOrientationWrench()`

---
 .../include/gazebo/ExternalWrench.hh          |   3 +-
 .../externalwrench/src/ApplyExternalWrench.cc |  21 ++--
 plugins/externalwrench/src/ExternalWrench.cc  | 111 +++++++++++++++++-
 3 files changed, 126 insertions(+), 9 deletions(-)

diff --git a/plugins/externalwrench/include/gazebo/ExternalWrench.hh b/plugins/externalwrench/include/gazebo/ExternalWrench.hh
index 813ff8ef0..4f64e0654 100644
--- a/plugins/externalwrench/include/gazebo/ExternalWrench.hh
+++ b/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();
 };
diff --git a/plugins/externalwrench/src/ApplyExternalWrench.cc b/plugins/externalwrench/src/ApplyExternalWrench.cc
index bbae72997..066711ffc 100644
--- a/plugins/externalwrench/src/ApplyExternalWrench.cc
+++ b/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,8 +114,8 @@ void ApplyExternalWrench::onReset()
     // Change the operation mode to default option 'single'
     this->m_rpcThread.m_mode = "single";
 
-    // Change the orientation to default option 'orient_base'
-    this->m_rpcThread.m_mode = "orient_base";
+    // Change the orientation to default option 'orient_global'
+    this->m_rpcThread.m_orient = "orient_global";
 
     // Reset wrench count
     this->m_rpcThread.wrenchCount = 0;
@@ -133,7 +140,7 @@ bool RPCServerThread::threadInit()
     this->m_mode = "single";
 
     // Set the default operation mode
-    this->m_orient = "orient_base";
+    this->m_orient = "orient_global";
 
     // Set wrench count default value
     this->wrenchCount = 0;
@@ -151,7 +158,7 @@ void RPCServerThread::run()
             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" );
-            this->m_reply.addString ( "Insert [orient_base] or [orient_local] to change the operation mode" );
+            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");
@@ -159,7 +166,7 @@ void RPCServerThread::run()
             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 ( "[duration]: (double) Duration of the applied force in seconds" );
-            this->m_reply.addString ( "Note: If orientation is set to [orient_base], 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() \
@@ -264,7 +271,7 @@ void RPCServerThread::run()
                         wrenchesVector.clear();
                     }
                 }
-                else if (command.get(0).asString() == "orient_base") {
+                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";
diff --git a/plugins/externalwrench/src/ExternalWrench.cc b/plugins/externalwrench/src/ExternalWrench.cc
index 4d9b32ce5..085b50a18 100644
--- a/plugins/externalwrench/src/ExternalWrench.cc
+++ b/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)
     {
@@ -183,6 +183,115 @@ void ExternalWrench::applyWrench()
     }
 }
 
+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]);
+
+        //Frames:
+        //World frame = W
+        //Link frame = L
+        //Link CoG frame = Lg
+        //Applied Forece frame = F
+
+        //Quantities:
+        //Rotation Matrix = R
+        //Transformation Matrix = T
+        //Quaternion = Q
+        //Pose 3d = P
+        //Position 3d = p
+
+        //Examples:
+        //W_T_L = the Transformation Matrix from the World frame to the Link frame
+        //Lg_Q_F = the Quaternion from the Link CoG frame to the Applied Force frame
+
+        ignition::math::Vector3d linkCoGPos = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied
+        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);
+        ignition::math::Quaterniond Lg_Q_F = Lg_T_F.Rotation();
+
+        ignition::math::Vector3d W_p_Lg = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied
+        ignition::math::Quaterniond W_Q_Lg = link->WorldCoGPose().Rot();
+        ignition::math::Matrix4d W_T_Lg = ignition::math::Matrix4d (link->WorldCoGPose());
+
+        link->AddRelativeForce(force);
+        link->AddRelativeTorque(torque);
+
+        ignition::math::Quaterniond W_Q_F = W_Q_Lg*Lg_Q_F;
+        ignition::math::Matrix4d W_T_F = W_T_Lg*Lg_T_F;
+
+        const ignition::math::Vector3d cylinderHalfLenght = ignition::math::Vector3d ( 0,0,-0.15 );
+
+        ignition::math::Pose3d linkCoGPose (W_T_F*cylinderHalfLenght, W_Q_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]);
+
+        //Frames:
+        //World frame = W
+        //Link frame = L
+        //Link CoG frame = Lg
+        //Applied Forece frame = F
+
+        //Quantities:
+        //Rotation Matrix = R
+        //Transformation Matrix = T
+        //Quaternion = Q
+        //Pose 3d = P
+        //Position 3d = p
+
+        //Examples:
+        //W_T_L = the Transformation Matrix from the World frame to the Link frame
+        //Lg_Q_F = the Quaternion from the Link CoG frame to the Applied Force frame
+
+        math::Vector3d linkCoGPos = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied
+        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);
+        math::Quaterniond forceOrientation = Lg_T_F.Rotation();
+
+        math::Vector3d W_p_Lg = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied
+        math::Quaterniond W_Q_Lg = link->WorldCoGPose().Rot();
+        math::Matrix4d W_T_Lg = math::Matrix4d (link->WorldCoGPose());
+
+        link->AddForce(W_Q_Lg.RotateVectorReverse(force));
+        link->AddTorque(W_Q_Lg.RotateVectorReverse(torque));
+
+        math::Quaterniond W_Q_F = W_Q_Lg*Lg_Q_F;
+        math::Matrix4d W_T_F = W_T_Lg*Lg_T_F;
+
+        const math::Vector3d cylinderHalfLenght = ignition::math::Vector3d ( 0,0,-0.15 );
+
+        math::Pose3d linkCoGPose (W_T_F*cylinderHalfLenght, W_Q_F);
+#endif
+
+#if GAZEBO_MAJOR_VERSION == 7
+        msgs::Set(visualMsg.mutable_pose(), linkCoGPose.Ign());
+#else
+        msgs::Set(visualMsg.mutable_pose(), linkCoGPose);
+#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::deleteWrench()
 {
     this->wrench.link_name.clear();

From 742b2a577fd1a0c6091a5de301435fb27c792894 Mon Sep 17 00:00:00 2001
From: Hosameldin <hosameldin.mohamed@iit.it>
Date: Tue, 28 Jan 2020 14:17:03 +0100
Subject: [PATCH 3/7] Make some modifications in the comments

---
 plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh | 2 +-
 plugins/externalwrench/src/ApplyExternalWrench.cc            | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh b/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh
index 0b7478b23..408083eca 100644
--- a/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh
+++ b/plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh
@@ -30,7 +30,7 @@ public:
     std::string         m_mode;
 
     // variable for orientation mode
-    // base link or local orientations
+    // global or local orientation modes
     std::string         m_orient;
 
     int                 wrenchCount;
diff --git a/plugins/externalwrench/src/ApplyExternalWrench.cc b/plugins/externalwrench/src/ApplyExternalWrench.cc
index 066711ffc..865011b2d 100644
--- a/plugins/externalwrench/src/ApplyExternalWrench.cc
+++ b/plugins/externalwrench/src/ApplyExternalWrench.cc
@@ -163,8 +163,8 @@ void RPCServerThread::run()
             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 refernce 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 refernce frame if [orient_local] is selected)" );
             this->m_reply.addString ( "[duration]: (double) Duration of the applied force in seconds" );
             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 );

From 6a7134de6a573ba1645a588551af74ec35f44cf8 Mon Sep 17 00:00:00 2001
From: Hosameldin <hosameldin.mohamed@iit.it>
Date: Tue, 28 Jan 2020 16:18:22 +0100
Subject: [PATCH 4/7] Address comments: modify the "help" response +
 CHANGELOG.md file

---
 CHANGELOG.md                                      | 5 +++++
 plugins/externalwrench/src/ApplyExternalWrench.cc | 6 +++---
 2 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 66ce1cc95..6d7c07e5d 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -3,6 +3,11 @@ All notable changes to this project will be documented in this file.
 
 The format of this document is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
+## [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).
+
 ## [3.3.0] - 2019-12-13
 
 ### Added
diff --git a/plugins/externalwrench/src/ApplyExternalWrench.cc b/plugins/externalwrench/src/ApplyExternalWrench.cc
index 865011b2d..42716d82f 100644
--- a/plugins/externalwrench/src/ApplyExternalWrench.cc
+++ b/plugins/externalwrench/src/ApplyExternalWrench.cc
@@ -157,14 +157,14 @@ void RPCServerThread::run()
             this->m_reply.addVocab ( yarp::os::Vocab::encode ( "many" ) );
             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" );
+            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 (or link refernce 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 refernce frame if [orient_local] is selected)" );
+            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: 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 );

From 0709086742f52c1d491dc9bac9fbdfb658c51c80 Mon Sep 17 00:00:00 2001
From: Hosameldin <hosameldin.mohamed@iit.it>
Date: Wed, 29 Jan 2020 09:45:42 +0100
Subject: [PATCH 5/7] Some minor changes

---
 plugins/externalwrench/src/ExternalWrench.cc | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/plugins/externalwrench/src/ExternalWrench.cc b/plugins/externalwrench/src/ExternalWrench.cc
index 085b50a18..af7ec6852 100644
--- a/plugins/externalwrench/src/ExternalWrench.cc
+++ b/plugins/externalwrench/src/ExternalWrench.cc
@@ -147,7 +147,7 @@ void ExternalWrench::applyGlobalOrientationWrench()
         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);
+        ignition::math::Pose3d W_p_F (linkCoGPos - rotation*ignition::math::Vector3d ( 0,0,.15 ), 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]);
@@ -161,13 +161,13 @@ void ExternalWrench::applyGlobalOrientationWrench()
         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::Quaterniond forceOrientation = rotation.Rotation();
-        math::Pose3d linkCoGPose (linkCoGPos - rotation*ignition::math::Vector3d ( 0,0,.15 ), forceOrientation);
+        math::Pose3d W_p_F (linkCoGPos - rotation*ignition::math::Vector3d ( 0,0,.15 ), forceOrientation);
 #endif
 
 #if GAZEBO_MAJOR_VERSION == 7
-        msgs::Set(visualMsg.mutable_pose(), linkCoGPose.Ign());
+        msgs::Set(visualMsg.mutable_pose(), W_p_F.Ign());
 #else
-        msgs::Set(visualMsg.mutable_pose(), linkCoGPose);
+        msgs::Set(visualMsg.mutable_pose(), W_p_F);
 #endif
 #if GAZEBO_MAJOR_VERSION >= 9
         msgs::Set(visualMsg.mutable_material()->mutable_ambient(), ignition::math::Color(color[0],color[1],color[2],color[3]));
@@ -226,9 +226,9 @@ void ExternalWrench::applyLocalOrientationWrench()
         ignition::math::Quaterniond W_Q_F = W_Q_Lg*Lg_Q_F;
         ignition::math::Matrix4d W_T_F = W_T_Lg*Lg_T_F;
 
-        const ignition::math::Vector3d cylinderHalfLenght = ignition::math::Vector3d ( 0,0,-0.15 );
+        const ignition::math::Vector3d cylinderHalfLength = ignition::math::Vector3d ( 0,0,-0.15 );
 
-        ignition::math::Pose3d linkCoGPose (W_T_F*cylinderHalfLenght, W_Q_F);
+        ignition::math::Pose3d W_p_F (W_T_F*cylinderHalfLength, W_Q_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]);
@@ -268,15 +268,15 @@ void ExternalWrench::applyLocalOrientationWrench()
         math::Quaterniond W_Q_F = W_Q_Lg*Lg_Q_F;
         math::Matrix4d W_T_F = W_T_Lg*Lg_T_F;
 
-        const math::Vector3d cylinderHalfLenght = ignition::math::Vector3d ( 0,0,-0.15 );
+        const math::Vector3d cylinderHalfLength = ignition::math::Vector3d ( 0,0,-0.15 );
 
-        math::Pose3d linkCoGPose (W_T_F*cylinderHalfLenght, W_Q_F);
+        math::Pose3d W_p_F (W_T_F*cylinderHalfLength, W_Q_F);
 #endif
 
 #if GAZEBO_MAJOR_VERSION == 7
-        msgs::Set(visualMsg.mutable_pose(), linkCoGPose.Ign());
+        msgs::Set(visualMsg.mutable_pose(), W_p_F.Ign());
 #else
-        msgs::Set(visualMsg.mutable_pose(), linkCoGPose);
+        msgs::Set(visualMsg.mutable_pose(), W_p_F);
 #endif
 #if GAZEBO_MAJOR_VERSION >= 9
         msgs::Set(visualMsg.mutable_material()->mutable_ambient(), ignition::math::Color(color[0],color[1],color[2],color[3]));

From c60fd0c21973623e635d0dc1c45998faa737ac6b Mon Sep 17 00:00:00 2001
From: Hosameldin <hosameldin.mohamed@iit.it>
Date: Sun, 16 Feb 2020 23:25:05 +0100
Subject: [PATCH 6/7] Address comments - organize the source code

---
 CHANGELOG.md                                  |   3 +
 .../externalwrench/src/ApplyExternalWrench.cc |  24 ++++-
 plugins/externalwrench/src/ExternalWrench.cc  | 102 ++++++++++--------
 3 files changed, 82 insertions(+), 47 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 15777d2d2..eeb598cf4 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -9,6 +9,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 - 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
diff --git a/plugins/externalwrench/src/ApplyExternalWrench.cc b/plugins/externalwrench/src/ApplyExternalWrench.cc
index 42716d82f..0118e73a9 100644
--- a/plugins/externalwrench/src/ApplyExternalWrench.cc
+++ b/plugins/externalwrench/src/ApplyExternalWrench.cc
@@ -276,16 +276,32 @@ void RPCServerThread::run()
 
                     this->m_message = command.get(0).asString() + " wrench orientation option set";
 
-                    // Not clearing previous wrenches!
-
+                    // 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();
 
                     this->m_message = command.get(0).asString() + " wrench orientation option set";
 
-                    // Not clearing previous wrenches!
-
+                    // 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 {
                     this->m_reply.clear();
diff --git a/plugins/externalwrench/src/ExternalWrench.cc b/plugins/externalwrench/src/ExternalWrench.cc
index af7ec6852..c4e8afa9e 100644
--- a/plugins/externalwrench/src/ExternalWrench.cc
+++ b/plugins/externalwrench/src/ExternalWrench.cc
@@ -147,7 +147,12 @@ void ExternalWrench::applyGlobalOrientationWrench()
         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 W_p_F (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,22 @@ void ExternalWrench::applyGlobalOrientationWrench()
 
         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 W_p_F (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(), W_p_F.Ign());
+        msgs::Set(visualMsg.mutable_pose(), cylinderPose.Ign());
 #else
-        msgs::Set(visualMsg.mutable_pose(), W_p_F);
+        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]));
@@ -191,92 +201,98 @@ void ExternalWrench::applyLocalOrientationWrench()
         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 Forece frame = F
+        //Applied Force frame = F
 
         //Quantities:
+        //Transformation = T
+        //Position 3d = P
         //Rotation Matrix = R
-        //Transformation Matrix = T
-        //Quaternion = Q
-        //Pose 3d = P
-        //Position 3d = p
-
+        
         //Examples:
-        //W_T_L = the Transformation Matrix from the World frame to the Link frame
-        //Lg_Q_F = the Quaternion from the Link CoG frame to the Applied Force frame
+        //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
 
-        ignition::math::Vector3d linkCoGPos = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied
+        //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);
-        ignition::math::Quaterniond Lg_Q_F = Lg_T_F.Rotation();
+        ignition::math::Quaterniond Lg_R_F = Lg_T_F.Rotation();
 
-        ignition::math::Vector3d W_p_Lg = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied
-        ignition::math::Quaterniond W_Q_Lg = link->WorldCoGPose().Rot();
+        //Transformation from World frame -> Link CoG (Lg) frame
         ignition::math::Matrix4d W_T_Lg = ignition::math::Matrix4d (link->WorldCoGPose());
+        ignition::math::Quaterniond W_R_Lg = W_T_Lg.Rotation();
 
-        link->AddRelativeForce(force);
-        link->AddRelativeTorque(torque);
-
-        ignition::math::Quaterniond W_Q_F = W_Q_Lg*Lg_Q_F;
+        //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_R_Lg*Lg_R_F;
 
+        //For the cylindrical shapes used to visualize the applied forces
         const ignition::math::Vector3d cylinderHalfLength = ignition::math::Vector3d ( 0,0,-0.15 );
 
-        ignition::math::Pose3d W_p_F (W_T_F*cylinderHalfLength, W_Q_F);
+        //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 Forece frame = F
+        //Applied Force frame = F
 
         //Quantities:
+        //Transformation = T
+        //Position 3d = P
         //Rotation Matrix = R
-        //Transformation Matrix = T
-        //Quaternion = Q
-        //Pose 3d = P
-        //Position 3d = p
-
+        
         //Examples:
-        //W_T_L = the Transformation Matrix from the World frame to the Link frame
-        //Lg_Q_F = the Quaternion from the Link CoG frame to the Applied Force frame
+        //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
 
-        math::Vector3d linkCoGPos = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied
+        //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);
-        math::Quaterniond forceOrientation = Lg_T_F.Rotation();
+        math::Quaterniond Lg_R_F = Lg_T_F.Rotation();
 
-        math::Vector3d W_p_Lg = link->WorldCoGPose().Pos(); // Get link's COG position where wrench will be applied
-        math::Quaterniond W_Q_Lg = link->WorldCoGPose().Rot();
+        //Transformation from World frame -> Link CoG (Lg) frame
         math::Matrix4d W_T_Lg = math::Matrix4d (link->WorldCoGPose());
+        math::Quaterniond W_R_Lg = W_T_Lg.Rotation();
 
-        link->AddForce(W_Q_Lg.RotateVectorReverse(force));
-        link->AddTorque(W_Q_Lg.RotateVectorReverse(torque));
-
-        math::Quaterniond W_Q_F = W_Q_Lg*Lg_Q_F;
+        //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_R_Lg*Lg_R_F;
 
-        const math::Vector3d cylinderHalfLength = ignition::math::Vector3d ( 0,0,-0.15 );
+        //For the cylindrical shapes used to visualize the applied forces
+        const math::Vector3d cylinderHalfLength = math::Vector3d ( 0,0,-0.15 );
 
-        math::Pose3d W_p_F (W_T_F*cylinderHalfLength, W_Q_F);
+        //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(), W_p_F.Ign());
+        msgs::Set(visualMsg.mutable_pose(), cylinderPose.Ign());
 #else
-        msgs::Set(visualMsg.mutable_pose(), W_p_F);
+        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]));

From c00ff7eaf8ec8d23cf01f18092ee5d5270da783c Mon Sep 17 00:00:00 2001
From: Hosameldin <hosameldin.mohamed@iit.it>
Date: Sun, 16 Feb 2020 23:48:38 +0100
Subject: [PATCH 7/7] some minor modifications

---
 plugins/externalwrench/src/ExternalWrench.cc | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/plugins/externalwrench/src/ExternalWrench.cc b/plugins/externalwrench/src/ExternalWrench.cc
index c4e8afa9e..1cfd587ad 100644
--- a/plugins/externalwrench/src/ExternalWrench.cc
+++ b/plugins/externalwrench/src/ExternalWrench.cc
@@ -227,15 +227,13 @@ void ExternalWrench::applyLocalOrientationWrench()
         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);
-        ignition::math::Quaterniond Lg_R_F = Lg_T_F.Rotation();
 
         //Transformation from World frame -> Link CoG (Lg) frame
         ignition::math::Matrix4d W_T_Lg = ignition::math::Matrix4d (link->WorldCoGPose());
-        ignition::math::Quaterniond W_R_Lg = W_T_Lg.Rotation();
 
         //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_R_Lg*Lg_R_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 );
@@ -272,15 +270,13 @@ void ExternalWrench::applyLocalOrientationWrench()
         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);
-        math::Quaterniond Lg_R_F = Lg_T_F.Rotation();
 
         //Transformation from World frame -> Link CoG (Lg) frame
         math::Matrix4d W_T_Lg = math::Matrix4d (link->WorldCoGPose());
-        math::Quaterniond W_R_Lg = W_T_Lg.Rotation();
 
         //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_R_Lg*Lg_R_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 );