Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Closed
wants to merge 8 commits into from
Closed

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

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
b2fad6c
Add the option of the frame to represent the applied wrench
HosameldinMohamed Jan 27, 2020
b674a93
Add the functions `applyGlobalOrientationWrench()` and `applyLocalOri…
HosameldinMohamed Jan 28, 2020
742b2a5
Make some modifications in the comments
HosameldinMohamed Jan 28, 2020
6a7134d
Address comments: modify the "help" response + CHANGELOG.md file
HosameldinMohamed Jan 28, 2020
bedcdf5
Merge branch 'devel' into feature/addOrientOption
HosameldinMohamed Jan 28, 2020
0709086
Some minor changes
HosameldinMohamed Jan 29, 2020
c60fd0c
Address comments - organize the source code
HosameldinMohamed Feb 16, 2020
c00ff7e
some minor modifications
HosameldinMohamed Feb 16, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ 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).

HosameldinMohamed marked this conversation as resolved.
Show resolved Hide resolved
## [3.3.0] - 2019-12-13
Expand Down
4 changes: 4 additions & 0 deletions plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
3 changes: 2 additions & 1 deletion plugins/externalwrench/include/gazebo/ExternalWrench.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
};
Expand Down
93 changes: 75 additions & 18 deletions plugins/externalwrench/src/ApplyExternalWrench.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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;

Expand All @@ -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;

Expand All @@ -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])" );
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is it one of the base/root frame or the world frame? The words base/root and global seem to not to go along well.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

you're right it's confusing. However the phrase "The default frame orientation is the one of the base/root frame" was written before and I didn't want to change it even though the applied force's frame seems to have the orientation of the global one.

Check out this little demo where I apply forces in X,Y and Z directions in global_orient mode:
Peek 2020-02-16 17-21

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() \
Expand Down Expand Up @@ -216,27 +231,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_global") {
this->m_orient = command.get(0).asString();

this->m_message = command.get(0).asString() + " wrench orientation option set";

// Not clearing previous wrenches!
HosameldinMohamed marked this conversation as resolved.
Show resolved Hide resolved

}
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();
Expand Down
119 changes: 114 additions & 5 deletions plugins/externalwrench/src/ExternalWrench.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -147,7 +147,7 @@ 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);
ignition::math::Pose3d W_p_F (linkCoGPos - rotation*ignition::math::Vector3d ( 0,0,.15 ), forceOrientation);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A pose can be identified with H or T. Here W_p_F is confusing as to a position and not a Pose.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

And why the variable naming W_p_F ? Anyways the force are applied to the CoG of the link right? Only the visual is offset by these magic numbers?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A pose can be identified with H or T. Here W_p_F is confusing as to a position and not a Pose.

Right, in the next commit I have changed the abbreviations to be more clear. Considering only Transformation T, Rotation R, or Position P.

And why the variable naming W_p_F ? Anyways the force are applied to the CoG of the link right? Only the visual is offset by these magic numbers?

I have changed the name of the variable to more clear and explicit name.

#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]);
Expand All @@ -161,13 +161,122 @@ void ExternalWrench::applyWrench()
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);
HosameldinMohamed marked this conversation as resolved.
Show resolved Hide resolved
#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]));
#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]);

//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
HosameldinMohamed marked this conversation as resolved.
Show resolved Hide resolved
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;
HosameldinMohamed marked this conversation as resolved.
Show resolved Hide resolved

const ignition::math::Vector3d cylinderHalfLength = ignition::math::Vector3d ( 0,0,-0.15 );
HosameldinMohamed marked this conversation as resolved.
Show resolved Hide resolved

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]);

//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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we consider here the UnitZ of the Gazebo world ? or the UnitZ of the link?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the current setting is OK (in Gazebo world). Since UnitZ is used to construct the transformation from the applied force frame to the Link CoG frame (Lg_T_F), which then is multiplied to the transformation between the world frame and the Link CoG frame:

        ignition::math::Matrix4d W_T_F = W_T_Lg*Lg_T_F;

In this line for example.

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 cylinderHalfLength = ignition::math::Vector3d ( 0,0,-0.15 );

math::Pose3d W_p_F (W_T_F*cylinderHalfLength, W_Q_F);
#endif

#if GAZEBO_MAJOR_VERSION == 7
msgs::Set(visualMsg.mutable_pose(), W_p_F.Ign());
#else
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]));
Expand Down