How to get the current steering?

[Urara Kono]

Hello,

Thank you so much for providing this wonderful simulator.

I'm new to Pychrono and I have a question.

I'm trying to modify demo_VEH_SteeringController.py so that the vehicle can accept steering velocity and acceleration commands. I want to convert these two commands into throttle, brake, and steering input. 

ChPathFillowerDriver has GetSteering to get the driver steering input, but how can I get the current actual steering angle so that I can implement a PID controller for the steering?

Or in this simulation, is it assumed that the driver steering input is always the same as the actual steering?

Best regards,

Urara

[HUZAIFA MUSTAFA UNJHAWALA]

Hello,

Do you want to get the angle by which the tire-wheel steers? GetSteering will only give you the normalized steering input (between -1 and 1) that the driver applies onto the steering wheel at the current time step. 

If you would like to get the tire-wheel steer angle, you could do something like this

            // Get the right wheel state
            auto state = vehicle.GetWheel(0, VehicleSide{RIGHT})->GetState();
            // Wheel normal (expressed in global frame)
            ChVector<> wheel_normal = state.rot.GetYaxis();

            // Terrain normal at wheel location (expressed in global frame)
            ChVector<> Z_dir = terrain.GetNormal(state.pos);

            // Longitudinal (heading) and lateral directions, in the terrain plane
            ChVector<> X_dir = Vcross(wheel_normal, Z_dir);
            X_dir.Normalize();
            ChVector<> Y_dir = Vcross(Z_dir, X_dir);

            // Tire reference coordinate system
            ChMatrix33<> rot;
            rot.Set_A_axis(X_dir, Y_dir, Z_dir);
            ChCoordsys<> tire_csys(state.pos, rot.Get_A_quaternion());

            // Express wheel normal in tire frame
            ChVector<> n = tire_csys.TransformDirectionParentToLocal(wheel_normal);

            // Wheel normal in the Vehicle frame
            ChVector<> n_v = vehicle.GetTransform().TransformDirectionParentToLocal(wheel_normal);

            // Toe-in
            auto toe_in_r = std::atan2(n_v.x(), n_v.y());

However, I do not understand what you mean by "steering velocity and acceleration", do you want to control the lateral velocity and acceleration of the vehicle?

[Urara Kono]

Hello,

Thank you so much for the reply!

By "steering velocity and acceleration", I meant the steering angular velocity of the tires and the longitudinal acceleration of the vehicle body.

I tried your code in Pychrono.

However, even though I set the driver's steering input to be always "1" so that the vehicle keeps turning anti-clock-wise, the toe_in_r of all four wheels kept decreasing as shown in "toe-in.png".

The trajectory is shown in "trajectory.png". The vehicle starts from (0,0) and keeps turning to the left. 

Maybe the toe-in obtained from your code is still in the global frame?

Is toe_in_r equivalent to "δ" in the screenshot of the vehicle model figure attached to this email?

I want to obtain this "δ".

Best regards,

Urara

[HUZAIFA MUSTAFA UNJHAWALA]

Hello Urara,

The code I provided for toe_in_r is what I suppose is the \delta from your image. However note that the vehicles in PyChrono are 4 wheeled, so you might need to make an approximation to get the \delta from the image (maybe take the average).

That being said, from your plot, it is surprising that the rear tires have a toe angle and the angle keeps decreasing. Can you please post here the snippet of code that generated the data for the plots? I can try taking a quick look and reproduce this on my end.

Best

Huzaifa

[Urara Kono]

Dear Huzaifa,

Thank you so much! This is the link to my code: https://github.com/UraraKono/pychrono-steering
Could you first look at the README file to get the overview of my code? Please run main.py.

Best regards,

Urara

--
You received this message because you are subscribed to a topic in the Google Groups "ProjectChrono" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/projectchrono/bGrK7NFxsBI/unsubscribe.
To unsubscribe from this group and all its topics, send an email to projectchron...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/projectchrono/235b7608-63bb-46a4-91e0-0f27f75e0e06n%40googlegroups.com.

--

Urara Kono, Ph.D. student

GRASP Laboratory

Electrical and Systems Engineering
University of Pennsylvania 

she/her/hers

[HUZAIFA MUSTAFA UNJHAWALA]

Hello Urara,

I made a mistake in my code above. Changing your toe-in function from

def get_toe_in(self, wheel_state_global):
    # Wheel normal expressed in global frame
    wheel_normal = wheel_state_global.rot.GetYaxis()
    # Terrain normal at wheel location expressed in global frame
    Z_dir = self.terrain.GetNormal(wheel_state_global.pos)
    # Longitudinal (heading) and lateral directions, in the terrain plane
    wheel_normal_np = np.array([wheel_normal.x, wheel_normal.y, wheel_normal.z])

    #Vcross cannot be found in pychrono. So I'm converting ChVectorD to numpy array and using np.cross
    Z_dir_np = np.array([Z_dir.x, Z_dir.y, Z_dir.z])
    X_dir_np = np.cross(wheel_normal_np, Z_dir_np)
    X_dir = chrono.ChVectorD(X_dir_np[0], X_dir_np[1], X_dir_np[2])
    X_dir.Normalize()
    X_dir_np = np.array([X_dir.x, X_dir.y, X_dir.z])  
    Y_dir_np = np.cross(Z_dir_np, X_dir_np)
    Y_dir = chrono.ChVectorD(Y_dir_np[0], Y_dir_np[1], Y_dir_np[2])
    rot = chrono.ChMatrix33D()
    rot.Set_A_axis(X_dir, Y_dir, Z_dir)
    tire_csys = chrono.ChCoordsysD(wheel_state_global.pos, rot.Get_A_quaternion())

    # Express wheel normal in tire frame
    n = tire_csys.TransformDirectionParentToLocal(wheel_normal)
    # print("n",n.x, n.y, n.z)

    # Wheel normal in the vehicle frame
    n_v = self.my_hmmwv.GetVehicle().GetTransform().TransformDirectionLocalToParent(wheel_normal)

    # Toe-in
    toe_in = math.atan2(n_v.x, n_v.y)

    return toe_in

To

def get_toe_in(self, wheel_state_global):

# Wheel normal expressed in global frame

wheel_normal = wheel_state_global.rot.GetYaxis()

# Terrain normal at wheel location expressed in global frame

Z_dir = self.terrain.GetNormal(wheel_state_global.pos)

# Longitudinal (heading) and lateral directions, in the terrain plane

wheel_normal_np = np.array([wheel_normal.x, wheel_normal.y, wheel_normal.z])

#Vcross cannot be found in pychrono. So I'm converting ChVectorD to numpy array and using np.cross

Z_dir_np = np.array([Z_dir.x, Z_dir.y, Z_dir.z])

X_dir_np = np.cross(wheel_normal_np, Z_dir_np)

X_dir = chrono.ChVectorD(X_dir_np[0], X_dir_np[1], X_dir_np[2])

X_dir.Normalize()

X_dir_np = np.array([X_dir.x, X_dir.y, X_dir.z])

Y_dir_np = np.cross(Z_dir_np, X_dir_np)

Y_dir = chrono.ChVectorD(Y_dir_np[0], Y_dir_np[1], Y_dir_np[2])

rot = chrono.ChMatrix33D()

rot.Set_A_axis(X_dir, Y_dir, Z_dir)

tire_csys = chrono.ChCoordsysD(wheel_state_global.pos, rot.Get_A_quaternion())

# Express wheel normal in tire frame

n = tire_csys.TransformDirectionParentToLocal(wheel_normal)

# print("n",n.x, n.y, n.z)

# Toe-in

toe_in = math.atan2(n.x, n.y)

return toe_in

The mistake is that the code unnecessarily converted the wheel normal back to the vehicle frame and used that for the toe-in computation. So as you rightly pointed out, the angles were in vehicle frame.

That being said, I realized that the approach I proposed is too convoluted, the following code also works

# Get wheel state

wheel_state_global = self.my_hmmwv.GetVehicle().GetWheel(0,0).GetState() #in global frame

# Express wheel normal in tire frame

wheel_normal = wheel_state_global.rot.GetYaxis()

n = self.my_hmmwv.GetVehicle().GetWheel(0,0).GetTransform().TransformDirectionParentToLocal(wheel_normal)

# toe_in = get_toe_in(self, wheel_state_global)

toe_in = math.atan2(n.x, n.y)

This directly gets the tire frame using GetTransform().
These modified functions produce the following plot, which I think is what we expect

Best Huzaifa

[Urara Kono]

Dear Huzaifa,

Thank you for checking my codes!

I corrected my codes, but now all my four wheels stay 0. In the plot you sent me, it also seems that all four wheels are 0 and only the driver input is 30 degrees. 

My code currently sets all wheel drive in init_vehicle() in chrono_env/utils.py. So shouldn't all four wheels be 0 at the beginning and go up to 30 degrees?

I attached the plot that only plots the front left wheel and driver's input. '1st_code.png' and '2nd_code.png' correspond to the two patterns of your suggested codes.

I additionally tried two other methods in step() in chrono_env.py. However, none of these gave the results that I expected. 

I calculated the Euler angle of the wheel with respect to the vehicle (chassis) frame.

They should give roll and pitch being always 0 and yaw being 30 degrees.

I pushed this code and the figures to the same repo again. https://github.com/UraraKono/pychrono-steering

Would you mind taking a look at my code again?

Best regards,

Urara

To view this discussion on the web visit https://groups.google.com/d/msgid/projectchrono/1730ba00-7e2a-43eb-b546-e82ce77b3293n%40googlegroups.com.

[HUZAIFA MUSTAFA UNJHAWALA]

Hello Urara,

Sorry for missing that the code was still not behaving as expected.

Could you try this piece of code

     wheel = self.my_hmmwv.GetVehicle().GetWheel(0, 0).GetSpindle()

     wheel_normal_abs = wheel.GetA().Get_A_Yaxis()

     wheel_normal_loc = self.my_hmmwv.GetChassisBody(

     ).TransformDirectionParentToLocal(wheel_normal_abs)

     wheel_angle = np.arctan2(

         wheel_normal_loc.y, wheel_normal_loc.x) - np.pi/2

     toe_in = wheel_angle

It seems to be giving the right plot for the left front tire. Its the same idea as before, except now we compute the angle with respect to the chassis body's heading. Attached is the plot I get. Also its better if we don't refer this angle as the "toe" angle because the "toe" angle is usually defined at 0 steer and is a static parameter.

Let me know if this looks okay

[Prakhar Gupta]

Hello,

Posting a question on a related topic here - about retrieving vehicle states.

I have been able to extract speed, positions, rotations etc. from chrono using some of the GetX() functions as below:

            auto marker_com = hmmwv.GetChassis()->GetMarkers()[1]->GetAbsCoord().pos;
            double current_spd = hmmwv.GetVehicle().GetSpeed();
            //ChQuaternion current_quat = hmmwv.GetVehicle().GetRot();
            //ChVector current_rot = current_quat.Q_to_Euler123();
            double current_yaw = hmmwv.GetVehicle().GetRot().Q_to_Euler123().z(); //radians

I have not yet come across an existing function that retrieves the current Yaw Rate (rad/sec) of the vehicle directly. Is there such a function already that I can utilize? Or is there an attribute that I can access for this?

Thanks!

Prakhar Gupta

PhD Student

Clemson University

[Radu Serban]

Prakhar,

 

You can use the functions ChFrameMoving:: GetWvel_loc() and ChFrameMoving::GetWvel_par() to get the angular velocity of a body, expressed in the local and absolute reference frames, respectively. 

 

For a ChBody in Chrono (centroidal reference frame), you can call those methods directly on the body object (since ChBody is a ChFrameMoving).

 

However, when for a vehicle’s chassis body (which is modeled as a ChBodyAuxRef; i.e. a body with a non-centroidal reference frame), you will want to use something like:

    auto w = vehicle. GetRefFrame().GetWvel_loc();

to get the angular velocity of the chassis reference frame expressed in the chassis reference frame.  Then get the component along the Z axis of that chassis reference frame to get the yaw rate.

 

I should maybe provide wrapper functions to get this type of information for a ChVehicle.

 

--Radu

--
You received this message because you are subscribed to the Google Groups "ProjectChrono" group.
To unsubscribe from this group and stop receiving emails from it, send an email to projectchron...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/projectchrono/10a6bc2f-5519-4a21-b4e3-ab79932c2765n%40googlegroups.com.

[Prakhar Gupta]

Radu,

Got it, thanks! Yes, it would definitely be easier to find with the wrapper, but this works too. I guess, once one knows where to look, modelling and simulation setup gets faster. :)