18 #include <gazebo/physics/physics.hh> 19 #include <gazebo/transport/transport.hh> 32 this->pitchPid.Init(5, 0, 0, 0, 0, 0.3, -0.3);
33 this->rollPid.Init(5, 0, 0, 0, 0, 0.3, -0.3);
34 this->yawPid.Init(1.0, 0, 0, 0, 0, 1.0, -1.0);
35 this->pitchCommand = 0.5*
M_PI;
36 this->rollCommand = 0;
42 physics::ModelPtr _model,
47 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
54 std::string yawJointName =
"cgo3_vertical_arm_joint";
55 this->yawJoint = this->model->GetJoint(yawJointName);
56 if (this->sdf->HasElement(
"joint_yaw"))
59 yawJointName = sdf->Get<std::string>(
"joint_yaw");
60 if (this->model->GetJoint(yawJointName))
62 this->yawJoint = this->model->GetJoint(yawJointName);
66 gzwarn <<
"joint_yaw [" << yawJointName <<
"] does not exist?\n";
71 gzerr <<
"GimbalControllerPlugin::Load ERROR! Can't get yaw joint '" 72 << yawJointName <<
"' " << endl;
75 std::string rollJointName =
"cgo3_horizontal_arm_joint";
76 this->rollJoint = this->model->GetJoint(rollJointName);
77 if (this->sdf->HasElement(
"joint_roll"))
80 rollJointName = sdf->Get<std::string>(
"joint_roll");
81 if (this->model->GetJoint(rollJointName))
83 this->rollJoint = this->model->GetJoint(rollJointName);
87 gzwarn <<
"joint_roll [" << rollJointName <<
"] does not exist?\n";
92 gzerr <<
"GimbalControllerPlugin::Load ERROR! Can't get roll joint '" 93 << rollJointName <<
"' " << endl;
97 std::string pitchJointName =
"cgo3_camera_joint";
98 this->pitchJoint = this->model->GetJoint(pitchJointName);
99 if (this->sdf->HasElement(
"joint_pitch"))
102 pitchJointName = sdf->Get<std::string>(
"joint_pitch");
103 if (this->model->GetJoint(pitchJointName))
105 this->pitchJoint = this->model->GetJoint(pitchJointName);
109 gzwarn <<
"joint_pitch [" << pitchJointName <<
"] does not exist?\n";
112 if (!this->pitchJoint)
114 gzerr <<
"GimbalControllerPlugin::Load ERROR! Can't get pitch joint '" 115 << pitchJointName <<
"' " << endl;
120 std::string imuSensorName =
"camera_imu";
121 if (this->sdf->HasElement(
"imu"))
124 imuSensorName = sdf->Get<std::string>(
"imu");
127 this->imuSensor = std::static_pointer_cast<sensors::ImuSensor>(
128 sensors::SensorManager::Instance()->GetSensor(imuSensorName));
130 if (!this->imuSensor)
132 gzerr <<
"GimbalControllerPlugin::Load ERROR! Can't get imu sensor '" 133 << imuSensorName <<
"' " << endl;
140 this->node = transport::NodePtr(
new transport::Node());
141 this->node->Init(this->model->GetWorld()->GetName());
143 this->lastUpdateTime = this->model->GetWorld()->GetSimTime();
146 std::string pitchTopic = std::string(
"~/") + this->model->GetName() +
148 this->pitchSub = this->node->Subscribe(pitchTopic,
151 std::string rollTopic = std::string(
"~/") + this->model->GetName() +
153 this->rollSub = this->node->Subscribe(rollTopic,
156 std::string yawTopic = std::string(
"~/") + this->model->GetName() +
158 this->yawSub = this->node->Subscribe(yawTopic,
162 this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
166 pitchTopic = std::string(
"~/") + this->model->GetName()
167 +
"/gimbal_pitch_status";
168 this->pitchPub = node->Advertise<gazebo::msgs::Any>(pitchTopic);
171 rollTopic = std::string(
"~/") + this->model->GetName()
172 +
"/gimbal_roll_status";
173 this->rollPub = node->Advertise<gazebo::msgs::Any>(rollTopic);
177 yawTopic = std::string(
"~/") + this->model->GetName()
178 +
"/gimbal_yaw_status";
179 this->yawPub = node->Advertise<gazebo::msgs::Any>(yawTopic);
181 gzmsg <<
"GimbalControllerPlugin::Init" << std::endl;
188 this->pitchCommand = _msg->double_value();
195 this->rollCommand = _msg->double_value();
202 this->yawCommand = _msg->double_value();
207 double r11,
double r12,
double r21,
double r31,
double r32)
209 return ignition::ignition::math::Vector3d
d(
217 const ignition::math::Quaterniond &_q)
222 ignition::ignition::math::Vector3d
result = this->ThreeAxisRot(
223 -2*(_q.X()*_q.Y() - _q.W()*_q.Z()),
224 _q.W()*_q.W() - _q.X()*_q.X() + _q.Y()*_q.Y() - _q.Z()*_q.Z(),
225 2*(_q.Y()*_q.Z() + _q.W()*_q.X()),
226 -2*(_q.X()*_q.Z() - _q.W()*_q.Y()),
227 _q.W()*_q.W() - _q.X()*_q.X() - _q.Y()*_q.Y() + _q.Z()*_q.Z());
234 if (!this->pitchJoint || !this->rollJoint || !this->yawJoint)
237 common::Time time = this->model->GetWorld()->GetSimTime();
238 if (time < this->lastUpdateTime)
240 gzerr <<
"time reset event\n";
241 this->lastUpdateTime = time;
244 else if (time > this->lastUpdateTime)
246 double dt = (this->lastUpdateTime - time).Double();
251 const double rDir = -1;
252 const double pDir = -1;
253 const double yDir = 1;
256 double rollLimited = ignition::math::clamp(this->rollCommand,
257 rDir*this->rollJoint->GetUpperLimit(0).Radian(),
258 rDir*this->rollJoint->GetLowerLimit(0).Radian());
259 double pitchLimited = ignition::math::clamp(this->pitchCommand,
260 pDir*this->pitchJoint->GetUpperLimit(0).Radian(),
261 pDir*this->pitchJoint->GetLowerLimit(0).Radian());
262 double yawLimited = ignition::math::clamp(this->yawCommand,
263 yDir*this->yawJoint->GetLowerLimit(0).Radian(),
264 yDir*this->yawJoint->GetUpperLimit(0).Radian());
266 ignition::math::Quaterniond commandRPY(
267 rollLimited, pitchLimited, yawLimited);
274 ignition::ignition::math::Vector3d currentAngleYPRVariable(
275 this->imuSensor->Orientation().Euler());
276 ignition::ignition::math::Vector3d currentAnglePRYVariable(
277 this->QtoZXY(currentAngleYPRVariable));
281 ignition::ignition::math::Vector3d lowerLimitsPRY
282 (pDir*this->pitchJoint->GetLowerLimit(0).Radian(),
283 rDir*this->rollJoint->GetLowerLimit(0).Radian(),
284 yDir*this->yawJoint->GetLowerLimit(0).Radian());
285 ignition::ignition::math::Vector3d upperLimitsPRY
286 (pDir*this->pitchJoint->GetUpperLimit(0).Radian(),
287 rDir*this->rollJoint->GetUpperLimit(0).Radian(),
288 yDir*this->yawJoint->GetUpperLimit(0).Radian());
291 double pitchError = this->ShortestAngularDistance(
292 pitchLimited, currentAnglePRYVariable.X());
293 double rollError = this->ShortestAngularDistance(
294 rollLimited, currentAnglePRYVariable.Y());
295 double yawError = this->ShortestAngularDistance(
296 yawLimited, currentAnglePRYVariable.Z());
307 if (lowerLimitsPRY.X() < upperLimitsPRY.X())
309 pitchError = ignition::math::clamp(pitchError,
310 currentAnglePRYVariable.X() - upperLimitsPRY.X(),
311 currentAnglePRYVariable.X() - lowerLimitsPRY.X());
315 pitchError = ignition::math::clamp(pitchError,
316 currentAnglePRYVariable.X() - lowerLimitsPRY.X(),
317 currentAnglePRYVariable.X() - upperLimitsPRY.X());
319 if (lowerLimitsPRY.Y() < upperLimitsPRY.Y())
321 rollError = ignition::math::clamp(rollError,
322 currentAnglePRYVariable.Y() - upperLimitsPRY.Y(),
323 currentAnglePRYVariable.Y() - lowerLimitsPRY.Y());
327 rollError = ignition::math::clamp(rollError,
328 currentAnglePRYVariable.Y() - lowerLimitsPRY.Y(),
329 currentAnglePRYVariable.Y() - upperLimitsPRY.Y());
331 if (lowerLimitsPRY.Z() < upperLimitsPRY.Z())
333 yawError = ignition::math::clamp(yawError,
334 currentAnglePRYVariable.Z() - upperLimitsPRY.Z(),
335 currentAnglePRYVariable.Z() - lowerLimitsPRY.Z());
339 yawError = ignition::math::clamp(yawError,
340 currentAnglePRYVariable.Z() - lowerLimitsPRY.Z(),
341 currentAnglePRYVariable.Z() - upperLimitsPRY.Z());
345 double pitchForce = this->pitchPid.Update(pitchError, dt);
346 this->pitchJoint->SetForce(0, pDir*pitchForce);
348 double rollForce = this->rollPid.Update(rollError, dt);
349 this->rollJoint->SetForce(0, rDir*rollForce);
351 double yawForce = this->yawPid.Update(yawError, dt);
352 this->yawJoint->SetForce(0, yDir*yawForce);
364 this->lastUpdateTime = time;
373 m.set_type(gazebo::msgs::Any_ValueType_DOUBLE);
375 m.set_double_value(this->pitchJoint->GetAngle(0).Radian());
376 this->pitchPub->Publish(m);
378 m.set_double_value(this->rollJoint->GetAngle(0).Radian());
379 this->rollPub->Publish(m);
381 m.set_double_value(this->yawJoint->GetAngle(0).Radian());
382 this->yawPub->Publish(m);
389 double diff = _angle - reference;
391 while (diff <= -
M_PI)
399 return diff + reference;
405 return this->NormalizeAbout(_to, _from) - _from;
void OnPitchStringMsg(ConstAnyPtr &_msg)
GimbalControllerPlugin()
Constructor.
ignition::ignition::math::Vector3d ThreeAxisRot(double r11, double r12, double r21, double r31, double r32)
void OnYawStringMsg(ConstAnyPtr &_msg)
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
double NormalizeAbout(double _angle, double _reference)
returns _angle1 normalized about (_reference - M_PI, _reference + M_PI]
static const bool kPrintOnPluginLoad
void OnRollStringMsg(ConstAnyPtr &_msg)
double ShortestAngularDistance(double _from, double _to)
returns shortest angular distance from _from to _to
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
ignition::ignition::math::Vector3d QtoZXY(const ignition::math::Quaterniond &_q)