Simulating suspension in rear wheel drive vehicle when turning

edit

Hi all,

I'm trying to simulate an ackerman driven medium vehicle including suspension. For the most part the simulation works, however when turning, I have a problem with one of the rear wheels occaisionally sending what seems to be a large impulse force up through the suspension. this force is large enough to not only move the wheel but also bounce the entire vehicle back end, such that it cause it to change direction. My SDF parameters for the rear wheels are (same for all four wheels):

<link name="left_rear_wheel">
    <collision name="left_rear_collision">
      <!--origin xyz="0.1 -0.13 0.1" rpy="0 1.5707 1.5707" /-->
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
     <geometry>
       <cylinder radius ="0.5" length= "0.335"/>
     </geometry>
    </collision>
    <gazebo reference ="left_rear_wheel">
              <kp>1e+9</kp>
              <kd>5e+6</kd>
               <mu>0.9</mu>
                <mu2>0.9</mu2>
              <slip1>1</slip1>
              <slip2>1</slip2>
              <fdir1>0 0 1</fdir1>
      <material>Gazebo/Orange</material>
    </gazebo>

The suspension is modelled in the plugin:

void updateSuspension()
        {
            double frontLeftForce = -frontLeftSuspension->GetAngle(0).Radian()*springConstant - frontLeftSuspension->GetVelocity(0)*dampingConstant;
            frontLeftSuspension->SetForce(0, frontLeftForce);

            double frontRightForce = -frontRightSuspension->GetAngle(0).Radian()*springConstant - frontRightSuspension->GetVelocity(0)*dampingConstant;
            frontRightSuspension->SetForce(0, frontRightForce);

            double rearLeftForce = -rearLeftSuspension->GetAngle(0).Radian()*springConstant - rearLeftSuspension->GetVelocity(0)*dampingConstant;
            rearLeftSuspension->SetForce(0, rearLeftForce);

            double rearRightForce = -rearRightSuspension->GetAngle(0).Radian()*springConstant - rearRightSuspension->GetVelocity(0)*dampingConstant;
            rearRightSuspension->SetForce(0, rearRightForce);
        }

And my rear wheel drive wheel drive torque is applied here, And in my plugin update I used some code from the drcsim vehicle to modify the cfm_stop attributes which I thought may be an issue.:

void updateRearWheelDrive(double speed)
        {

            double controlValue = speedController.ControlAlgorithm(speed, rightRear->GetVelocity(0), 100, 1000 ,0);

            if(speed!= 0)
            {
                this->rightRear->SetAttribute("stop_cfm", 0, 1.0);
                this->leftRear->SetAttribute("stop_cfm", 0, 1.0);
                this->frontRightWheel->SetAttribute("stop_cfm", 0, 1.0);
                this->frontLeftWheel->SetAttribute("stop_cfm", 0, 1.0);
            }
            else
            {
                this->rightRear->SetAttribute("stop_cfm", 0, 0.0);
                this->leftRear->SetAttribute("stop_cfm", 0, 0.0);
                this->frontRightWheel->SetAttribute("stop_cfm", 0, 0.0);
                this->frontLeftWheel->SetAttribute("stop_cfm", 0, 0.0);
            }

            rightRear->SetForce(0, controlValue);
            leftRear->SetForce(0, controlValue);
            frontLeftWheel->SetForce(0, 0);
            frontRightWheel->SetForce(0, 0);
        }

I think the problem may be due to a collision force being generated by the ground plane and the inside rear wheel. This problem only seems to be apparent when turning and particularly so when driving forward. I am just wondering if there is some collision attribute for the wheels that may alleviate any built up forces on the tires that is causing this? I have played around with the slip value but it doesn't really seem to do much.