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

edit

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 ...