37 #include <Eigen/Geometry> 38 #include <ignition/math/Vector3.hh> 39 #include <ignition/math/Quaternion.hh> 40 #include <ignition/math/Pose3.hh> 41 #include <gazebo/msgs/wrench_stamped.pb.h> 44 inline Eigen::Matrix<double,6,1>&
gazeboMsgToEigen(const ::gazebo::msgs::WrenchStamped& w, Eigen::Matrix<double,6,1>& e_out)
46 e_out[0] = w.wrench().force().x();
47 e_out[1] = w.wrench().force().y();
48 e_out[2] = w.wrench().force().z();
49 e_out[3] = w.wrench().torque().x();
50 e_out[4] = w.wrench().torque().y();
51 e_out[5] = w.wrench().torque().z();
55 inline Eigen::Matrix<double,6,1>
gazeboMsgToEigen(const ::gazebo::msgs::WrenchStamped& w)
57 Eigen::Matrix<double,6,1> e;
61 inline Eigen::Quaterniond
quatFromRPY(
double roll,
double pitch,
double yaw )
63 return Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
64 * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
65 * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
69 inline Eigen::Vector3d
convVec3(
const ignition::math::Vector3d &_vec3)
71 return Eigen::Vector3d(_vec3.X(),_vec3.Y(),_vec3.Z());
74 inline Eigen::Vector3d
convVec3(
const gazebo::msgs::Vector3d &_vec3)
76 return Eigen::Vector3d(_vec3.x(),_vec3.y(),_vec3.z());
79 inline ignition::math::Vector3d
convVec3(
const Eigen::Vector3d &_vec3)
81 return ignition::math::Vector3d(_vec3[0], _vec3[1], _vec3[2]);
84 inline Eigen::Quaterniond
convQuat(
const ignition::math::Quaterniond &_quat)
86 return Eigen::Quaterniond(_quat.W(), _quat.X(), _quat.Y(), _quat.Z());
89 inline ignition::math::Quaterniond
convQuat(
const Eigen::Quaterniond &_quat)
91 return ignition::math::Quaterniond(_quat.w(), _quat.x(), _quat.y(), _quat.z());
94 inline Eigen::Affine3d
convPose(
const ignition::math::Pose3d &_pose)
98 res.translation() =
convVec3(_pose.Pos());
99 res.linear() =
convQuat(_pose.Rot()).toRotationMatrix();
104 inline ignition::math::Pose3d
convPose(
const Eigen::Affine3d &_T)
106 ignition::math::Pose3d pose;
107 pose.Pos() =
convVec3(_T.translation());
108 pose.Rot() =
convQuat(Eigen::Quaterniond(_T.linear()));
117 FILE* pipe = popen(_cmd.c_str(),
"r");
123 std::string result =
"";
127 if (fgets(buffer, 128, pipe) != NULL)
159 : base_link_(base_link)
182 connection_ptr_ = conn_ptr;
200 ::gazebo::event::ConnectionPtr connection_ptr_;
201 std::string base_link_;
202 Eigen::Matrix<double,6,1> wrench_;
218 void connectUpdate(std::function<
void (::gazebo::msgs::WrenchStamped)> subscriber)
220 if (sensor_ !=
nullptr)
222 sensor_->ConnectUpdate(subscriber)
230 void setSensor(::gazebo::sensors::ForceTorqueSensorPtr sensor)
235 ::gazebo::sensors::ForceTorqueSensorPtr
getSensor()
const 246 ::gazebo::sensors::ForceTorqueSensorPtr sensor_ =
nullptr;
259 if (sensor_ !=
nullptr)
261 sensor_->ConnectUpdated(subscriber)
273 void setSensor(::gazebo::sensors::ContactSensorPtr sensor)
299 if (link_ ==
nullptr)
300 throw std::runtime_error(
"Link not set.");
301 ::gazebo::msgs::Contacts contacts = this->getSensor()->Contacts();
304 Eigen::Matrix<double,6,1> wrench_res_atCoG_inLink = Eigen::Matrix<double,6,1>::Zero();
305 auto linkCoGPose = this->getLink()->GetInertial()->Pose();
306 std::string scoped_collision_name = link_->GetModel()->GetName() +
"::" + this->
getBaseLinkName() +
"::" + this->getCollisionName();
307 for (
unsigned int i = 0; i < contacts.contact_size(); ++i)
309 for (
unsigned int j = 0; j < contacts.contact(i).wrench_size(); ++j)
311 if (0 == contacts.contact(i).wrench(j).body_1_name().compare(scoped_collision_name))
313 wrench_res_atCoG_inLink.head<3>() +=
convVec3(contacts.contact(i).wrench(j).body_1_wrench().force());
314 wrench_res_atCoG_inLink.tail<3>() +=
convVec3(contacts.contact(i).wrench(j).body_1_wrench().torque());
316 else if (0 == contacts.contact(i).wrench(j).body_2_name().compare(scoped_collision_name))
318 wrench_res_atCoG_inLink.head<3>() +=
convVec3(contacts.contact(i).wrench(j).body_2_wrench().force());
319 wrench_res_atCoG_inLink.tail<3>() +=
convVec3(contacts.contact(i).wrench(j).body_2_wrench().torque());
322 throw std::runtime_error(
"Mismatching collision bodies names");
329 ::gazebo::sensors::ContactSensorPtr sensor_ =
nullptr;
330 ::gazebo::physics::LinkPtr link_ =
nullptr;
331 std::string collision_;
const Eigen::Matrix< double, 6, 1 > & getWrench() const
returns the wrench in the base_link frame
Definition: Utils.h:166
void connectUpdate(std::function< void(::gazebo::msgs::WrenchStamped)> subscriber)
Definition: Utils.h:218
GazeboForceTorqueSensor(::gazebo::physics::LinkPtr link)
Definition: Utils.h:213
void setSensor(::gazebo::sensors::ForceTorqueSensorPtr sensor)
sets the ForceTorqueSensorPtr to connect to
Definition: Utils.h:230
Definition: GazeboServer.h:59
void setWrench(const Eigen::Matrix< double, 6, 1 > &wrench)
sets the wrench in the base_link frame
Definition: Utils.h:189
Eigen::Matrix< double, 6, 1 > & wrench()
Definition: Utils.h:194
Eigen::Quaterniond quatFromRPY(double roll, double pitch, double yaw)
Definition: Utils.h:61
::gazebo::sensors::ForceTorqueSensorPtr getSensor() const
Definition: Utils.h:235
const std::string & getBaseLinkName()
returns the wrench in the base_link name, from the world to the body
Definition: Utils.h:174
std::string custom_exec(const std::string &_cmd)
Definition: Utils.h:115
GazeboWrenchSensor()
Definition: Utils.h:151
GazeboWrenchSensor(const std::string &base_link)
Definition: Utils.h:158
Eigen::Vector3d convVec3(const ignition::math::Vector3d &_vec3)
Definition: Utils.h:69
void setWrenchFrom(const ::gazebo::msgs::WrenchStamped &wrench_stamped)
Definition: Utils.h:240
GazeboForceTorqueSensor(const std::string &base_link)
Definition: Utils.h:209
virtual ~GazeboWrenchSensor()
Definition: Utils.h:154
Definition: CartesianAccelerationPID.h:44
Eigen::Matrix< double, 6, 1 > & gazeboMsgToEigen(const ::gazebo::msgs::WrenchStamped &w, Eigen::Matrix< double, 6, 1 > &e_out)
Definition: Utils.h:44
Eigen::Quaterniond convQuat(const ignition::math::Quaterniond &_quat)
Definition: Utils.h:84
void setConnection(::gazebo::event::ConnectionPtr conn_ptr)
Definition: Utils.h:180
Eigen::Affine3d convPose(const ignition::math::Pose3d &_pose)
Definition: Utils.h:94
A Wrench sensor w.r.t. a base_link. Wrench is expressed from the world to the body, in the base_link frame.
Definition: Utils.h:148
Eigen::Matrix< double, 6, 1 > & transportWrench(const Eigen::Matrix< double, 6, 1 > &wrench_in_a, const Eigen::Matrix< double, 3, 1 > &b_to_a, Eigen::Matrix< double, 6, 1 > &wrench_in_b_out)
Definition: Utils.h:97