Skip to content

Commit

Permalink
change attachForceTorqueSensorToJoint and attachContactSensorToLink
Browse files Browse the repository at this point in the history
  • Loading branch information
@iadev
iadev committed Sep 22, 2018
1 parent 3ace3ea commit 35c239f
Show file tree
Hide file tree
Showing 2 changed files with 45 additions and 15 deletions.
15 changes: 10 additions & 5 deletions examples/gazebo/07-orca_gazebo_wrench_task.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,16 +105,21 @@ int main(int argc, char const *argv[])

auto ft_sensor_j6 = gz_model.attachForceTorqueSensorToJoint("ati_joint");
// Connect gazebo force torque sensor to the wrench_task
auto c = ft_sensor_j6->ConnectUpdate([&](::gazebo::msgs::WrenchStamped w)
ft_sensor_j6->connectUpdate([&](::gazebo::msgs::WrenchStamped w)
{
Vector6d current_wrench = gazeboMsgToEigen(w);
wrench_task->setCurrentWrenchValue( current_wrench );
ft_sensor_j6->setWrenchFrom(w);
wrench_task->setCurrentWrenchValue( ft_sensor_j6->getWrench() );
std::cout << " Desired Wrench: " << desired_wrench.transpose() << '\n';
std::cout << " Current Wrench: " << current_wrench.transpose() << '\n';
std::cout << " Current Wrench: " << ft_sensor_j6->getWrench().transpose() << '\n';
});


auto contact_j6 = gz_model.attachContactSensorToLink("link_7");
auto contact_j6 = gz_model.attachContactSensorToLink("ati_link", "ati_link_fixed_joint_lump__collision_ati_link_collision");
contact_j6->connectUpdate([&]()
{
contact_j6->setWrenchFrom();
std::cout << "Contact Wrench:" << contact_j6->getWrench().transpose() << std::endl;
});

auto jnt_trq_cstr = controller.addConstraint<JointTorqueLimitConstraint>("JointTorqueLimit");
jnt_trq_cstr->setLimits(Eigen::VectorXd::Constant(ndof,-200.),Eigen::VectorXd::Constant(ndof,200.));
Expand Down
45 changes: 35 additions & 10 deletions include/orca/gazebo/GazeboModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,7 +227,8 @@ class GazeboModel
return true;
}

::gazebo::sensors::ForceTorqueSensorPtr attachForceTorqueSensorToJoint(const std::string& joint_name,double update_rate = 0)

std::shared_ptr<orca::gazebo::GazeboForceTorqueSensor> attachForceTorqueSensorToJoint(const std::string& joint_name,double update_rate = 0)
{
assertModelLoaded();

Expand All @@ -241,8 +242,15 @@ class GazeboModel
ss << " <update_rate>" << update_rate << "</update_rate>";
ss << " <always_on>true</always_on>";
ss << " <visualize>true</visualize>";
ss << " <force_torque>";
ss << " <measure_direction>child_to_parent</measure_direction>";
ss << " <frame>child</frame>";
ss << " </force_torque>";
ss << " </sensor>";
ss << "</sdf>";
auto sensor_base_link = joint->GetChild(); // this is related to the <frame>child</frame> setting in the SDF. Forced.
auto sensor = std::make_shared<orca::gazebo::GazeboForceTorqueSensor>(sensor_base_link);

std::string forceTorqueSensorString = ss.str();
// Create the SDF file to configure the sensor
sdf::ElementPtr sdf(new sdf::Element);
Expand All @@ -262,13 +270,15 @@ class GazeboModel
mgr->Update();

// Get a pointer to the force torque sensor
auto sensor = std::dynamic_pointer_cast<::gazebo::sensors::ForceTorqueSensor>(
auto gz_sensor = std::dynamic_pointer_cast<::gazebo::sensors::ForceTorqueSensor>(
mgr->GetSensor(sensorName));

// Make sure the above dynamic cast worked.
if(!sensor) throw std::runtime_error("Could not create force_torque sensor for joint " + joint_name);
if(!gz_sensor) throw std::runtime_error("Could not create force_torque sensor for joint " + joint_name);

if(!sensor->IsActive()) throw std::runtime_error("Sensor is not active");
if(!gz_sensor->IsActive()) throw std::runtime_error("Sensor is not active");

sensor->setSensor(gz_sensor);

std::cout << "[GazeboModel \'" << getName() << "\'] " << "Force torque sensor '" << sensorName << "' successfully created" << '\n';
return sensor;
Expand All @@ -277,22 +287,35 @@ class GazeboModel
#endif
}

::gazebo::sensors::ContactSensorPtr attachContactSensorToLink(const std::string& link_name,double update_rate = 0)
std::shared_ptr<orca::gazebo::GazeboContactSensor> attachContactSensorToLink(const std::string& link_name, const std::string& collision_name, double update_rate = 0)
{
assertModelLoaded();

#if GAZEBO_MAJOR_VERSION > 8
auto link = model_->GetLink(link_name);
if(!link)
if (!link)
throw std::runtime_error("Link " + link_name + " does not exists");
auto collision = link->GetChildCollision(collision_name);
if (!collision)
{
std::string error = "Collision " + collision_name + " does not exist in link " + link_name + ".\nCandidates are:\n";
for (const auto& coll : link->GetCollisions())
error += " - " + coll->GetName() + "\n";
throw std::runtime_error(error);
}
std::stringstream ss;
ss << "<sdf version='1.4'>";
ss << " <sensor name='contact' type='contact'>";
ss << " <update_rate>" << update_rate << "</update_rate>";
ss << " <always_on>true</always_on>";
ss << " <visualize>true</visualize>";
ss << " <contact>";
ss << " <collision>" << collision_name << "</collision>";
ss << " </contact>";
ss << " </sensor>";
ss << "</sdf>";
auto sensor = std::make_shared<orca::gazebo::GazeboContactSensor>(link, collision_name);

std::string contactSensorStr = ss.str();
// Create the SDF file to configure the sensor
sdf::ElementPtr sdf(new sdf::Element);
Expand All @@ -312,21 +335,23 @@ class GazeboModel
mgr->Update();

// Get a pointer to the force torque sensor
auto sensor = std::dynamic_pointer_cast<::gazebo::sensors::ContactSensor>(
auto gz_sensor = std::dynamic_pointer_cast<::gazebo::sensors::ContactSensor>(
mgr->GetSensor(sensorName));

// Make sure the above dynamic cast worked.
if(!sensor) throw std::runtime_error("Could not create contact sensor for link " + link_name);
if(!gz_sensor) throw std::runtime_error("Could not create contact sensor for link " + link_name);

if(!gz_sensor->IsActive()) throw std::runtime_error("Sensor is not active");

if(!sensor->IsActive()) throw std::runtime_error("Sensor is not active");
sensor->setSensor(gz_sensor);

std::cout << "[GazeboModel \'" << getName() << "\'] " << "Contact sensor '" << sensorName << "' successfully created" << '\n';
return sensor;
#else
throw std::runtime_error("Adding sensors is only supported for gz version > 8");
#endif
}

const Eigen::Vector3d& getGravity() const
{
assertModelLoaded();
Expand Down

4 comments on commit 35c239f

@ahoarau
Copy link
Member

Choose a reason for hiding this comment

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

Nice ! Does it work ?

@iadev
Copy link
Collaborator Author

@iadev iadev commented on 35c239f Sep 22, 2018 •
edited

Choose a reason for hiding this comment

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

Yes it does !
Returned wrench is in link frame for both sensors, child link for GazeboForceTorqueSensor.
You can get the link in question using my_sensor->getBaseLinkName().

Still, for the ContactSensor, I inferred that the JointWrenchs in Gazebo contact messages are in link frame, at the CoG. This is not what is stated in the Gazebo documentation, though.
I submitted a comment on Gazebo repo here, waiting for confirmation.
A note in the code documentation is there.

So far both sensors appear to be consistent with each other, so this is good news.
I'll upload a simple pendulum example later to orca_demos to demonstrate that.

@traversaro
Copy link

@traversaro traversaro commented on 35c239f Sep 23, 2018 •
edited

Choose a reason for hiding this comment

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

I submitted a comment on Gazebo repo here, waiting for confirmation.

Hi @iadev ! I think a comment on an old commit whose author is not working anymore on Gazebo has quite a low visibility. I would suggest to either open a new issue, or even better do a pull fixing documentation on the Gazebo side (this is typical the way for which it is more probable to get feedback).

Regarding the FT frame, you are using <frame>child</frame> so you should not have any problem, but you may be nevertheless be interested in this issue: https://bitbucket.org/osrf/sdformat/issues/130/position-part-of-force_torque-sensor-pose .

@iadev
Copy link
Collaborator Author

@iadev iadev commented on 35c239f Sep 23, 2018

Choose a reason for hiding this comment

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

Hello @traversaro, thanks for the follow up !

I didn't take the time to file an issue yet, so I dropped the comment to keep track of it in the meantime.

Concerning the FR frame, yes that's exactly why I forced the use of <frame>child</frame>, to avoid any confusion/issues.
Thanks for the link to the issue #130, indeed it's interesting to be aware of that !

Please sign in to comment.