Gazebo | Ignition | Community
Ask Your Question
0

Joints Ros-Gazebo

asked 2012-11-06 12:57:50 -0500

pmarinplaza gravatar image

Hello,

I want to know how the joints work with gazebo. My problem is that I don't know what kind of physics laws are used for gazebo.

I am using one plugin for control the wheels on (4 wheel) differential car. I am using one class in the pkg pr2_controller/manager and one of my question is the next:

In this plugin, there are 3 states, star, update and stop. I configure update to 50 Hz. Every time to run this state, I send efforts to joints by this way:

std::string joint_blw;
  if (!n.getParam("joint_blw", joint_blw))
  {
    ROS_ERROR("No joint BLW given in namespace: '%s')",
              n.getNamespace().c_str());
    return false;
  }
  joint_state_blw_ = robot->getJointState(joint_blw);
  if (!joint_state_blw_)
  {
    ROS_ERROR("summit_xl_ctrl could not find joint named '%s'",
              joint_blw.c_str());
    return false;
  }
  ROS_DEBUG("summit_xl_ctrl found joint named '%s'", joint_blw.c_str());
...
...
joint_state_blw_->commanded_effort_ = ...

This joint is continuous and has this urdf:

.. ..

  <joint name="joint_back_left_wheel" type="continuous">
    <parent link="base_link"/>
    <child link="back_left_wheel"/>
    <origin xyz="-0.223 0.154 0" rpy="0 0 0" /> 
    <axis xyz="0 1 0" rpy="0 0 0" />
    <limit effort="100" velocity="100"/>
    <joint_properties damping="0.0" friction="0.0"/>
  </joint>

  <transmission name="joint_blw_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="joint_blw_motor" />
     <joint name="joint_back_left_wheel" />
     <mechanicalReduction>1</mechanicalReduction>
     <motorTorqueConstant>1</motorTorqueConstant>
  </transmission>

Finally, I don't know where is this effort apply and what is the performance of this effort, like I send torque to make par or whatever in N or Nm?. Where can I find any documentation or tutorial about this?

By the Way this is for the Uc3m University

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2012-11-21 06:54:23 -0500

PMilani gravatar image

updated 2012-11-21 07:05:59 -0500

pmarinplaza

I copied an example from erraticrobot diffdriveplugin.cpp. It should be in your opt/ros/stacks/erraticrobot/ erraticrobot_plugins/ directory or something like that.

Essentially they created a pointer to a joint in their plugin using: physics::JointPtr joint; joint = this->model->getJoint("joint_name");

then in the update method applied a torque to that joint through:

joint->SetForce(0, forceinNorNm);

where the force is in N or Nm depending on the type of joint. This method seems to work well. I would expect were you to subscribe to a topic that generated your torques then put them in your SetForce method, that should work. Please let me know.

BTW I don't think SetVelocity actually does anything in Gazebo but I would like enlightenment if that is not the case. When ever i post a value to it nothing seems to happen.

cheers

Peter

edit flag offensive delete link more

Comments

Very nice answer, I'll see that code and see what I can use. In my case, I am using pr2 manager to control the joints and I want to know if I can control the joint in velocity (like rpm) because torque force depends on weight. Thank you very much ;)

pmarinplaza gravatar imagepmarinplaza ( 2012-11-21 09:28:59 -0500 )edit

If you want to control velocity, your plugin will have to do dynamics conversions. You will need a state space representation of your drive/transmission that converts your control signal into torque or force. If you observe the velocity (joint->getVelocity(0)) you can adjust your control signal.

PMilani gravatar imagePMilani ( 2012-11-29 01:08:31 -0500 )edit

@pmarinplaza, mark this as the correct answer if you believe it is. Doing so will give you and @PMilani to karma so that you will be able to vote on questions and answers and improve the system.

asomerville gravatar imageasomerville ( 2012-11-29 13:29:42 -0500 )edit

@PMilani: In theory ODE"s dParamVel will try to set the desired velocity but limited by dParamFMax. Have you tried setting dParamFMax to a larger value?

hsu gravatar imagehsu ( 2012-11-29 18:23:05 -0500 )edit

@hsu, unfortunately no, it may be the problem if the default of dParamFMax is zero. Though I do like having to convert electricity or pressure to force as this is really the basis for all mechanical movement. Also it would be helpful if GetForce() were implemented in Gazebo for force sensing.

PMilani gravatar imagePMilani ( 2012-12-02 17:50:46 -0500 )edit
0

answered 2013-12-26 04:42:02 -0500

Saras Arya gravatar image

updated 2013-12-26 04:46:58 -0500

I am trying to use Set Force on my model in update function i checked Force is getting Set but the vehicle is not moving my update function is public: void Update(const common::UpdateInfo &) { physics::JointPtr MyJoint = this->model->GetJoint("leftwheelhinge"); MyJoint->SetForce(0,1.00); physics::JointPtr MyJoint2=this->model->GetJoint("rightwheelhinge"); MyJoint2->SetForce(0,1.00); }

edit flag offensive delete link more

Comments

this needs a new question, however, try checking your force and velocity limits for your joint.

PMilani gravatar imagePMilani ( 2014-01-19 19:34:43 -0500 )edit

Question Tools

Stats

Asked: 2012-11-06 12:57:50 -0500

Seen: 2,920 times

Last updated: Dec 26 '13