74 ROS_FATAL_STREAM_NAMED(
"hand_of_god",
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 75 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
80 if(_sdf->HasElement(
"robotNamespace")) {
84 if(_sdf->HasElement(
"frameId")) {
85 this->
frame_id_ = _sdf->Get<std::string>(
"frameId");
88 if(_sdf->HasElement(
"kl")) {
89 this->
kl_ = _sdf->Get<
double>(
"kl");
91 if(_sdf->HasElement(
"ka")) {
92 this->
ka_ = _sdf->Get<
double>(
"ka");
95 if(_sdf->HasElement(
"linkName")) {
96 this->
link_name_ = _sdf->Get<std::string>(
"linkName");
111 const std::vector<physics::LinkPtr> &links =
model_->GetLinks();
112 for(
unsigned i=0; i < links.size(); i++) {
118 #if GAZEBO_MAJOR_VERSION >= 8 141 geometry_msgs::TransformStamped hog_desired_tform;
142 static bool errored =
false;
155 const geometry_msgs::Vector3 &p = hog_desired_tform.transform.translation;
156 const geometry_msgs::Quaternion &q = hog_desired_tform.transform.rotation;
157 ignition::math::Pose3d hog_desired(
158 ignition::math::Vector3d(p.x, p.y, p.z),
159 ignition::math::Quaterniond(q.w, q.x, q.y, q.z));
162 #if GAZEBO_MAJOR_VERSION >= 8 164 ignition::math::Vector3d worldLinearVel =
floating_link_->WorldLinearVel();
165 ignition::math::Vector3d relativeAngularVel =
floating_link_->RelativeAngularVel();
167 ignition::math::Pose3d world_pose =
floating_link_->GetDirtyPose().Ign();
168 ignition::math::Vector3d worldLinearVel =
floating_link_->GetWorldLinearVel().Ign();
169 ignition::math::Vector3d relativeAngularVel =
floating_link_->GetRelativeAngularVel().Ign();
171 ignition::math::Vector3d err_pos = hog_desired.Pos() - world_pose.Pos();
173 ignition::math::Quaterniond err_rot = (ignition::math::Matrix4d(world_pose.Rot()).Inverse()
174 * ignition::math::Matrix4d(hog_desired.Rot())).Rotation();
175 ignition::math::Quaterniond not_a_quaternion = err_rot.Log();
178 kl_ * err_pos -
cl_ * worldLinearVel);
181 ka_ * ignition::math::Vector3d(not_a_quaternion.X(), not_a_quaternion.Y(), not_a_quaternion.Z())
182 -
ca_ * relativeAngularVel);
185 geometry_msgs::TransformStamped hog_actual_tform;
187 hog_actual_tform.header.frame_id =
"world";
190 hog_actual_tform.child_frame_id =
frame_id_ +
"_actual";
192 hog_actual_tform.transform.translation.x = world_pose.Pos().X();
193 hog_actual_tform.transform.translation.y = world_pose.Pos().Y();
194 hog_actual_tform.transform.translation.z = world_pose.Pos().Z();
196 hog_actual_tform.transform.rotation.w = world_pose.Rot().W();
197 hog_actual_tform.transform.rotation.x = world_pose.Rot().X();
198 hog_actual_tform.transform.rotation.y = world_pose.Rot().Y();
199 hog_actual_tform.transform.rotation.z = world_pose.Rot().Z();
GazeboRosHandOfGod()
Constructor.
#define ROS_ERROR_STREAM_NAMED(name, args)
event::ConnectionPtr update_connection_
Pointer to the update event connection.
ROSCPP_DECL bool isInitialized()
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
boost::shared_ptr< tf2_ros::Buffer > tf_buffer_
#define ROS_FATAL_STREAM_NAMED(name, args)
physics::LinkPtr floating_link_
std::string robot_namespace_
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
virtual ~GazeboRosHandOfGod()
Destructor.
#define ROS_ERROR_NAMED(name,...)
boost::shared_ptr< tf2_ros::TransformBroadcaster > tf_broadcaster_
boost::shared_ptr< tf2_ros::TransformListener > tf_listener_
virtual void GazeboUpdate()
Update the controller.