follow the tutorial about Gazebo SetVelocity Plugin but err with Assertion `px != 0' failed. [closed]
Hi, I just started learning gazebo plugin, follow the tutorials "Setting Velocity on Joints" and test my own robot. but there is Problem:
my robot description:
<?xml version="1.0" ?>
<sdf version="1.7">
<world name="the_forest_world">
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://ground_plane</uri>
</include>
<!-- A testing model -->
<model name='robot_forest'>
<include>
<uri>model://robot_forest</uri>
</include>
<plugin name="SetJointVelocityPlugin" filename="libSetJointVelocityPlugin.so"/>
</model>
</world>
</sdf>
my SetVelocity c++ plugin:
#include <gazebo/gazebo.hh>
#include <gazebo/physics/Joint.hh>
#include <gazebo/physics/JointController.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/PhysicsTypes.hh>
namespace gazebo
{
//////////////////////////////////////////////////
/// \brief Sets velocity on a link or joint
class SetJointVelocityPlugin : public ModelPlugin
{
public: physics::JointControllerPtr jointController;
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
this->model = _model;
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&SetJointVelocityPlugin::Update, this, std::placeholders::_1));
}
public: void Update(const common::UpdateInfo &_info)
{
if (update_num == 0)
{
// Joint velocity instantaneously without applying forces
//this->model->GetJoint("revolute_joint_1_2l")->SetVelocity(0, 1.0);
// Joint velocity using joint motors
this->model->GetJoint("revolute_joint_1_2r")->SetParam("fmax", 0, 100.0);
this->model->GetJoint("revolute_joint_1_2r")->SetParam("vel", 0, 1.0);
// Joint velocity using PID controller
this->jointController.reset(new physics::JointController(
this->model));
this->jointController->AddJoint(model->GetJoint("revolute_joint_1_2l"));
std::string name = model->GetJoint("revolute_joint_1_2l")->GetScopedName();
this->jointController->SetVelocityPID(name, common::PID(100, 0, 0));
this->jointController->SetVelocityTarget(name, 1.0);
}
else if (update_num < 200)
{
// Must update PID controllers so they apply forces
this->jointController->Update();
}
else if (update_num == 200)
{
// Joint motors are disabled by setting max force to zero
this->model->GetJoint("revolute_joint_1_2r")->SetParam("fmax", 0, 0.0);
}
update_num++;
}
/// \brief a pointer to the model this plugin was loaded by
public: physics::ModelPtr model;
/// \brief object for callback connection
public: event::ConnectionPtr updateConnection;
/// \brief number of updates received
public: int update_num = 0;
};
GZ_REGISTER_MODEL_PLUGIN(SetJointVelocityPlugin)
}
and the err:
gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:734: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = gazebo::physics::Joint; typename boost::detail::sp_member_access<T>::type = gazebo::physics::Joint*]: Assertion `px != 0' failed.
could anyone help me, what is wrong?
thanks a lot