Manipulating ContactPlugin Data

[osrf-migration]

Original report (archived issue) by Anonymous.

The original report had attachments: Screenshot from 2012-12-18 19:03:19.png, Screenshot from 2012-12-18 19:03:41.png, Screenshot from 2012-12-18 19:04:57.png

---

Situation: Trying to write a plugin to determine exernal contact forces on a link and publish to a ROS msg.

Status: I have a custom SensorPlugin class written which takes the associated Contact sensor and I can access all the contact data from the gazebo msgs::Contacts. I've verified this some simple printfs.

Next Steps: I'd like to accumulate all external forces/torques and publish a single external force wrench in the link coord frame to a ROS msg. Publishing to Ros is fairly straightforward and there are a number of good code examples to go on. My current issue is with manipulating data coming from the sensor.

In my ContactPlugin load fn I grab a ptr to the physics::Link

#!c++
this->world_ = physics::get_world(this->sensor_->GetWorldName());
physics::EntityPtr ent = this->world_->GetEntity(this->sensor_->GetParentName());
physics::Link * lnk = dynamic_cast<physics::Link*>(ent.get());
this->link_.reset(lnk);

In my update function I can grab current pose and contact data from sensor as follows:

#!c++
msgs::Contacts contacts = this->sensor_->GetContacts();
math::Pose pose = this->link_->GetWorldPose();

First question:
What cardinal point on my link does the pose correspond to, i.e. a point of (0,0,0) in the link frame would be where? At the cg or proximal at the output of the parent joint?

Second question:
I could iterate through all contacts accumulating forces in the link frame

#!c++
Vector3 tot_force;
Vector3 tot_moment;
for (int i = 0; i < contacts.contact_size(); ++i)
{
	//transorm force and moment to link frame
	tot_foce += new_force;
	tot_moment += new_moment;
}

However, this seems like something that would've already been done by your physics engine and I ought to be able to access the accumulated results. There are some force/torque accessors as members of physics::Link

#!c++
Vector3 torque = this->link_->GetWorldForce();
Vector3 torque = this->link_->GetWorldTorque();

However, the results always seem to be zeros. Am I on the right track with this, is there an easier way?

Many thanks,

[osrf-migration]

Original comment by Benjamin Swilling (Bitbucket: swillb).

---

Here's a print out of some accumulated force data:

data generated this way:

#!c++
		math::Vector3 f_tot;
		math::Vector3 m_tot;
		for (int i = 0; i < contacts.contact_size(); ++i)
		{
			for (int j = 0; j < contacts.contact(i).position_size(); ++j)
			{
				f_tot.x += contacts.contact(i).wrench(j).body_1_force().x();
				f_tot.y += contacts.contact(i).wrench(j).body_1_force().y();
				f_tot.z += contacts.contact(i).wrench(j).body_1_force().z();
			}
		}

		gzmsg << this->link_->GetName() << " f_tot [" << this->world_->GetSimTime().Double() << "]:  " << f_tot << std::endl;

prints out this:

Msg l_foot f_tot [2.53]:  323.075 -56.7376 710.48
Msg l_foot f_tot [2.531]:  323.075 -56.7376 710.48
Msg l_foot f_tot [2.532]:  323.075 -56.7376 710.48
Msg l_foot f_tot [2.533]:  323.075 -56.7376 710.48
Msg l_foot f_tot [2.535]:  323.075 -56.7376 710.48
Msg l_foot f_tot [2.536]:  323.075 -56.7376 710.48
Msg l_foot f_tot [2.537]:  323.075 -56.7376 710.48
Msg l_foot f_tot [2.538]:  323.075 -56.7376 710.48
Msg l_foot f_tot [2.539]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.541]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.542]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.543]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.544]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.546]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.547]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.549]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.55]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.551]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.552]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.554]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.555]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.556]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.557]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.558]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.559]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.561]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.562]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.563]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.564]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.566]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.567]:  88.2868 116.337 280.305
Msg l_foot f_tot [2.568]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.569]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.571]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.572]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.573]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.574]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.575]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.576]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.577]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.578]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.579]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.581]:  -94.8786 -26.6833 5.92986
Msg l_foot f_tot [2.582]:  -94.8786 -26.6833 5.92986

Some of that data seems reasonable, some not. Also it seems I'm only getting updates at around ~30Hz (many repeated force values). I'd expect to see forces around (0,0,500) for a 1000N robot just standing there not doing much.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

Regarding the 30 Hz rate, can you check the model sdf to see whether the contact sensor element has an "update_rate" parameter (units are Hz)? This parameter serves to throttle the contact sensor. If it is not set, then the sensor should update with every simulation step. You can also force an update on every time step by calling SetUpdateRate(0) on the contact sensor object.

Let me know if this helps. If not, I'll take another look.

[osrf-migration]

Original comment by Benjamin Swilling (Bitbucket: swillb).

---

The "update_rate" seemed like a likely culprit, but there was no update_rate set in my urdf file. I did try setting the update_rate through the sensor in my code to something reasonably high (I think 1kHz) however that did not change the behavior.

It's also worth noting that when I ran my simulation plant at 8kHz this problem went away.

[osrf-migration]

Original comment by John Hsu (Bitbucket: hsu, GitHub: hsu).

---

The update rate of 30Hz is because sensors are updated in a loop, which each Sensors::run_once calls individual sensor updates serially, where the combined serial update rate is most likely limited to around 30Hz by image sensor updates due to image generation and memcopy from gpu to cpu memory.

[osrf-migration]

Original comment by John Hsu (Bitbucket: hsu, GitHub: hsu).

---

Also, each contact message contains array of contacts with unique time stamps, so summation needs to sum only the contacts within the same time step. I've created a patch (see gazebo pull request #199) and drcsim pull request #50. Together, they produce more reasonable plots (see attached).

[osrf-migration]

Original comment by John Hsu (Bitbucket: hsu, GitHub: hsu).

---

• set attachment to "Screenshot from 2012-12-18 19:04:57.png"

attaching plots from joint force torque readings (/drc_robot/r_foot_ft) and contact forces (/drc_robot/r_foot_contact) for standing drc robot.

[osrf-migration]

Original comment by John Hsu (Bitbucket: hsu, GitHub: hsu).

---

• changed state from "new" to "resolved"

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

• set version to "all"

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

• changed state from "resolved" to "closed"