follow the tutorial about Gazebo SetVelocity Plugin but err with Assertion `px != 0' failed. [closed]

asked 2020-08-06 04:19:33 -0500

zzq6258688 gravatar image

updated 2020-08-06 11:28:02 -0500

sloretz gravatar image

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

edit retag flag offensive reopen merge delete

Closed for the following reason Gazebo Question: The Gazebo community prefers to answer questions at: http://answers.gazebosim.org by gvdhoorn
close date 2020-08-06 13:08:56.018251