RE: [chrono] Re: How to get the current steering?

['Radu Serban' via ProjectChrono]

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

From: projectchrono@googlegroups.com <projectchrono@googlegroups.com> On Behalf 
Of Prakhar Gupta
Sent: Wednesday, January 24, 2024 10:49 PM
To: ProjectChrono <projectchrono@googlegroups.com>
Subject: Re: [chrono] Re: How to get the current steering?

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
On Monday 13 November 2023 at 14:31:52 UTC-5 
unjh...@wisc.edu<mailto:unjh...@wisc.edu> wrote:
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
On Thursday, November 9, 2023 at 2:32:45 PM UTC-6 ura...@seas.upenn.edu wrote:
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

On Thu, Nov 9, 2023 at 9:59 AM 'HUZAIFA MUSTAFA UNJHAWALA' via ProjectChrono 
<projec...@googlegroups.com<mailto:projec...@googlegroups.com>> wrote:
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

On Wednesday, November 8, 2023 at 11:02:54 AM UTC-6 
ura...@seas.upenn.edu<mailto:ura...@seas.upenn.edu> wrote:
Dear Huzaifa,

Thank you so much! This is the link to my code: 
https://github.com/UraraKono/pychrono-steering<https://urldefense.com/v3/__https:/github.com/UraraKono/pychrono-steering__;!!IBzWLUs!VjMQTjj7XYOajqdEp2lng6LR8VoirumLVvQh9bxVJkIHsL9lfVWBopcU5pDozX8ZAUT0sVo0oEycxoQ5TyAhcEl1btSDG3s$>
Could you first look at the README file to get the overview of my code? Please 
run main.py.

Best regards,
Urara

On Mon, Nov 6, 2023 at 1:02 PM 'HUZAIFA MUSTAFA UNJHAWALA' via ProjectChrono 
<projec...@googlegroups.com<mailto:projec...@googlegroups.com>> wrote:
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

On Friday, November 3, 2023 at 11:03:56 AM UTC-5 
ura...@seas.upenn.edu<mailto:ura...@seas.upenn.edu> wrote:
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



On Wednesday, November 1, 2023 at 9:55:12 AM UTC-4 
unjh...@wisc.edu<mailto:unjh...@wisc.edu> wrote:
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?





On Monday, October 23, 2023 at 9:23:40 AM UTC-5 
ura...@seas.upenn.edu<mailto:ura...@seas.upenn.edu> wrote:
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
--
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<https://urldefense.com/v3/__https:/groups.google.com/d/topic/projectchrono/bGrK7NFxsBI/unsubscribe__;!!IBzWLUs!VEreeTZm1KgpDeiWYmCTDRYWhiKJqfzXDEKYU59kbFDJ_2ZcUm7gnZiztNrlSdmlWbNvWiYj1gv6pD9ZozBOiFn11u021xw$>.
To unsubscribe from this group and all its topics, send an email to 
projectchron...@googlegroups.com<mailto: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<https://urldefense.com/v3/__https:/groups.google.com/d/msgid/projectchrono/235b7608-63bb-46a4-91e0-0f27f75e0e06n*40googlegroups.com?utm_medium=email&utm_source=footer__;JQ!!IBzWLUs!VEreeTZm1KgpDeiWYmCTDRYWhiKJqfzXDEKYU59kbFDJ_2ZcUm7gnZiztNrlSdmlWbNvWiYj1gv6pD9ZozBOiFn1bxQTjlE$>.


--
Urara Kono, Ph.D. student
GRASP Laboratory<https://www.grasp.upenn.edu>
Electrical and Systems Engineering
University of Pennsylvania
she/her/hers
--
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<https://urldefense.com/v3/__https:/groups.google.com/d/topic/projectchrono/bGrK7NFxsBI/unsubscribe__;!!IBzWLUs!VjMQTjj7XYOajqdEp2lng6LR8VoirumLVvQh9bxVJkIHsL9lfVWBopcU5pDozX8ZAUT0sVo0oEycxoQ5TyAhcEl1qA0Ruoc$>.
To unsubscribe from this group and all its topics, send an email to 
projectchron...@googlegroups.com<mailto:projectchron...@googlegroups.com>.
To view this discussion on the web visit 
https://groups.google.com/d/msgid/projectchrono/1730ba00-7e2a-43eb-b546-e82ce77b3293n%40googlegroups.com<https://urldefense.com/v3/__https:/groups.google.com/d/msgid/projectchrono/1730ba00-7e2a-43eb-b546-e82ce77b3293n*40googlegroups.com?utm_medium=email&utm_source=footer__;JQ!!IBzWLUs!VjMQTjj7XYOajqdEp2lng6LR8VoirumLVvQh9bxVJkIHsL9lfVWBopcU5pDozX8ZAUT0sVo0oEycxoQ5TyAhcEl1KkAg4tw$>.


--
Urara Kono, Ph.D. student
GRASP Laboratory<https://www.grasp.upenn.edu>
Electrical and Systems Engineering
University of Pennsylvania
she/her/hers
--
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 
projectchrono+unsubscr...@googlegroups.com<mailto:projectchrono+unsubscr...@googlegroups.com>.
To view this discussion on the web visit 
https://groups.google.com/d/msgid/projectchrono/10a6bc2f-5519-4a21-b4e3-ab79932c2765n%40googlegroups.com<https://groups.google.com/d/msgid/projectchrono/10a6bc2f-5519-4a21-b4e3-ab79932c2765n%40googlegroups.com?utm_medium=email&utm_source=footer>.

-- 
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 projectchrono+unsubscr...@googlegroups.com.
To view this discussion on the web visit 
https://groups.google.com/d/msgid/projectchrono/PH0PR06MB8237A41F935071173592AD0CA77A2%40PH0PR06MB8237.namprd06.prod.outlook.com.