Home | Tutorials | Wiki | Issues
Ask Your Question
2

How to set a joint position and and a joint force at the same time? Is it feasible with the current API?

asked 2016-11-04 09:39:39 -0600

Andrei gravatar image

updated 2016-11-14 02:12:53 -0600

Hi,

I am building a vehicle model with 2 revolute joints for each wheel. One joint for steering and one for spinning. I am working on the plugin that is attached to the model. When I set a position to the steering joint and a force to the spinning joint during the same OnUpdate event, the SetPosition seems to cancel out the resulting motion due to the Force I set on the other joint (spinning joint). I can't figure out why this is the case. Can anyone give me some more insight?

Thanks, Andrei

P.S. I am using Gazebo-7. I have added the model and (part of) the code from my plugin below.

Code sample:

void VehiclePlugin::OnUpdate(const common::UpdateInfo & /*_info*/)
{

    update_counter = update_counter+1;

    if ( update_counter>100 &&
         update_counter<=300 ) {// appy a torque
          double torque_joint = 50000;
        SetWheelDriveTorque(WheelNames::CENTER_LEFT,torque_joint);
        SetWheelDriveTorque(WheelNames::CENTER_RIGHT,torque_joint);
    }

     SetWheelSteerAngle(WheelNames::FRONT_LEFT,0.2);
     SetWheelSteerAngle(WheelNames::FRONT_RIGHT,0.2);
     SetWheelSteerAngle(WheelNames::CENTER_LEFT,0.2);
     SetWheelSteerAngle(WheelNames::CENTER_RIGHT,0.2);
     SetWheelSteerAngle(WheelNames::REAR_LEFT,0.2);
     SetWheelSteerAngle(WheelNames::REAR_RIGHT,0.2);


     (...)
}

void VehiclePlugin::SetWheelDriveTorque(const WheelNames & wheel_name,  const double & torque_value)
{
   // try to find wheel joint for this wheelname
   if ( m_jointsptr_map.count(wheel_name)>0) {
        physics::JointPtr wheel_joint_pointer  = m_jointsptr_map[wheel_name];

        wheel_joint_pointer->SetForce(1,torque_value);
   }

}

void VehiclePlugin::SetWheelSteerAngle(const WheelNames & wheel_name, const double & angle_value)
{
    // try to find wheel joint for this wheelname
    if ( m_jointsptr_map.count(wheel_name)>0 ) {
        physics::JointPtr wheel_joint_pointer  = m_jointsptr_map[wheel_name];

        if ( !wheel_joint_pointer->SetPosition(0,angle_value) ) {
             std::cerr << ">>> WARNING: Could not set joint axis angle/position" << std::endl;
        }
     }
 }

SDF model below (for a six-wheeled vehicle)

<model name="sixwheeled_vehicle">
        <link name='lumped_chassis'>
        <pose>0 0 5.16 0 0 0</pose>
            <gravity>1</gravity>
            <inertial>
                <mass>54800</mass>
                <inertia>
                    <ixx>308297.493333</ixx>
                    <iyy>417897.493333</iyy>
                    <izz>337933.333333</izz>
                    <ixy>0</ixy>
                    <ixz>0</ixz>
                    <iyz>0</iyz>
                </inertia>
            </inertial>
        <visual name='lumped_chassis'>
            <geometry>
                <box>
                    <size>7 5 6.52</size>
                </box>
            </geometry>
        </visual>
        <collision name='collision_pseudo_chassis'>
            <geometry>
                <box>
                    <size>7 5 6.52</size>
                </box>
            </geometry>
        </collision>
    </link>
    <link name="wheel_left_center">
        <pose>0 2.266 0.76 -1.5707963267949 0 0</pose>
        <gravity>1</gravity>
        <inertial>
            <mass>50</mass>
            <inertia>
                <ixx>7.88666666667</ixx>
                <iyy>7.88666666667</iyy>
                <izz>14.44</izz>
                <ixy>0</ixy>
                <ixz>0</ixz>
                <iyz>0</iyz>
            </inertia>
        </inertial>
        <collision name="collision_WLC">
            <geometry>
                <cylinder>
                    <radius>0.76</radius>
                    <length>0.4</length>
                </cylinder>
            </geometry>
            <surface>
                <friction>
                    <torsional>
                        <coefficient>0.0</coefficient>
            <surface_radius>0.01</surface_radius>
            <use_patch_radius>false</use_patch_radius>
                    </torsional>
                    <ode>
                        <fdir1>1 0 0</fdir1>
                        <mu>1.0</mu>
                        <mu2>0.0</mu2>
                        <slip1>0</slip1>
                        <slip2>0</slip2>
                    </ode>
                </friction>
                <contact>
                    <ode>
          <min_depth>0.005</min_depth>
          <kp>1e8</kp>
        </ode>
                </contact>
        </surface>
        </collision>
        <visual name="visual_WLC">
            <geometry>
                <cylinder>
                    <radius>0.76</radius>
                    <length>0.4</length>
                </cylinder>
            </geometry>
            <material>
                <script>
                    <uri>file://media/materials/scripts/gazebo.material</uri>
                    <name>Gazebo/Black</name>
                </script>
            </material>
        </visual>
    </link>
    <link name="wheel_left_front">
        <pose>3.85 2.266 0.76 -1.5707963267949 0 0</pose>
        <gravity>1</gravity>
        <inertial>
            <mass>50</mass>
            <inertia>
                <ixx ...
(more)
edit retag flag offensive close merge delete

Comments

Can you post your code, model, and specify what version of gazebo you are using?

nkoenig gravatar imagenkoenig ( 2016-11-07 09:45:25 -0600 )edit

@nkoenig: I have edited my question with the SDF model and the code. If the code is not sufficient, let me know and I can send the entire file. (m_jointsptr_map is a map object containing mapping <wheelname, jointptr=""> which is filled in at Load time. I have printed out the child link's velocities after the call to SetPosition and indeed the child gets 0 velocities after that call. Is there something I am doing wrong?

Andrei gravatar imageAndrei ( 2016-11-08 03:01:44 -0600 )edit

In general, you don't want to use `SetPosition` on a joint. The `SetPosition` function bypasses the physics engine, and performs some teleportation of child links. You should apply forces to joints and write a controller to achieve desired joint angles.

nkoenig gravatar imagenkoenig ( 2016-11-09 10:29:06 -0600 )edit

Ok. I see, thank you. I will try it with PID controller. I have seen an example here http://answers.gazebosim.org/question/2341/set-and-get-position-of-gazebo-model-using-ros-plugin/ Thanks again.

Andrei gravatar imageAndrei ( 2016-11-09 11:54:56 -0600 )edit

@nkoenig: I guess I will have to write my own implementation of JointController::Update method for my steer axis controller of the universal joint, as from what I see in the Gazebo source code, the JointController:: Update method only updates the 0th axis of the joint and does not support an axis index.

Andrei gravatar imageAndrei ( 2016-11-10 04:16:13 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-11-14 03:34:15 -0600

Andrei gravatar image

updated 2016-11-14 03:34:29 -0600

Based on nkoenig's suggestion and an answer by AndreiHaidu to another question (see http://answers.gazebosim.org/question...) my solution that seems to work so far is:

  • created a PID object for each joint I want to control
  • created an Update method for the controller (based on link above and the Update method in JointController.cc of Gazebo-7 source code) , code snippet given below
    • finally, in the the OnUpdate step of my plugin class i set my desired steering angle set-point (m_setpoint) via a different function and call the Update method I implemented for all my joints' PIDs.

void PID_JointController::Update(){

common::Time currTime = this->m_model->GetWorld()->GetSimTime();
common::Time stepTime = currTime - this->m_prevUpdateTime;
this->m_prevUpdateTime = currTime;

if ( stepTime > 0 ) {
    m_cmd_force = this->m_PositionPID.Update(
                this->m_JointPtr->GetAngle(m_axis_index).Radian() -
                this->m_setpoint, stepTime);
    this->m_JointPtr->SetForce(m_axis_index, m_cmd_force);
}
edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2016-11-04 09:39:39 -0600

Seen: 2,021 times

Last updated: Nov 14 '16