ChLinkMotorRotationSpeed reaction torque

[Simon]

Hi,

as per the documentation, ChLinkMotorRotationSpeed's member functions Get_react_force() and Get_react_torque() return the respective values in link coordinates.
As I understand, the link coordinate system is the 'master' body system (Body2).

In the example below a body rotates around the absolute z axis using ChLinkMotorRotationSpeed (the 'master' body is fixed here). As expected, the link reaction force consists of a the constant body weight in z direction and the rotating centrifugal load in the x-y plane. However, the reaction torque (due to gravity) is constant despite the link's rotation. Shouldn't the torque vector also rotate or do I misunderstand something?

Thanks in advance!

Simon

Code:

ChSystemNSC system{};

auto fixed_body{chrono_types::make_shared<ChBody>()};
fixed_body->SetBodyFixed(true);
auto rotating_body{chrono_types::make_shared<ChBody>()};
rotating_body->SetPos({1.0, 0.0, 0.0});

auto rotation_link{chrono_types::make_shared<ChLinkMotorRotationSpeed>()};
rotation_link->Initialize(rotating_body, fixed_body, ChFrame{});
rotation_link->SetSpeedFunction(chrono_types::make_shared<ChFunction_Const>(1.0));

system.Add(fixed_body);
system.Add(rotating_body);
system.Add(rotation_link);

system.Set_G_acc({0.0, 0.0, 10.0});

for (double time{}; time < 1.0; time += 0.01)
{
    system.DoFrameDynamics(time);

    std::cout << std::fixed << std::setprecision(3) << "Time: " << system.GetChTime()
              << " *** Force: " << rotation_link->Get_react_force()
              << " *** Moment: " << rotation_link->Get_react_torque() << "\n";
}

Output:
...

Time: 0.810 *** Force: 0.697  0.717  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.820 *** Force: 0.690  0.724  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.830 *** Force: 0.682  0.731  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.840 *** Force: 0.675  0.738  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.850 *** Force: 0.667  0.745  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.860 *** Force: 0.660  0.751  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.870 *** Force: 0.652  0.758  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.880 *** Force: 0.645  0.764  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.890 *** Force: 0.637  0.771  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.900 *** Force: 0.629  0.777  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.910 *** Force: 0.622  0.783  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.920 *** Force: 0.614  0.790  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.930 *** Force: 0.606  0.796  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.940 *** Force: 0.598  0.802  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.950 *** Force: 0.590  0.808  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.960 *** Force: 0.582  0.813  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.970 *** Force: 0.574  0.819  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.980 *** Force: 0.565  0.825  10.000 *** Moment: 0.000  -10.000  -0.000
Time: 0.990 *** Force: 0.557  0.831  10.000 *** Moment: 0.000  -10.000  -0.000

[Luning Fang]

Hello Simon,

"As I understand, the link coordinate system is the 'master' body system (Body2)."

This is not correct. Your link coordinate system is defined here,

rotation_link->Initialize(rotating_body, fixed_body, ChFrame{});

ChFrame{} initialize a reference frame coincide with the global reference frame. Because your Body2 (fixed_body) has the same orientation as the GRF, and it is fixed, it just so happens that your link coordinate system is the same as the 'master' body system. 

You can check the orientation of the  link coordinate system using this: 

auto quat = rotation_link->GetLinkAbsoluteCoords().rot

Thank you,

Luning

[Simon]

Hi Luning,

thanks for the clarification about the link coordinate system.
In my example I added the link coordinate system output to verify that it is indeed fixed to the global reference frame (see below).

Unfortunately my question remains: Why does the link force vector rotate and the torque vector does not ? 

Thank you,
Simon

Code:

ChSystemNSC system{};

auto fixed_body{chrono_types::make_shared<ChBody>()};
fixed_body->SetBodyFixed(true);
auto rotating_body{chrono_types::make_shared<ChBody>()};
rotating_body->SetPos({1.0, 0.0, 0.0});

auto rotation_link{chrono_types::make_shared<ChLinkMotorRotationSpeed>()};
rotation_link->Initialize(rotating_body, fixed_body, ChFrame{});
rotation_link->SetSpeedFunction(chrono_types::make_shared<ChFunction_Const>(1.0));

system.Add(fixed_body);
system.Add(rotating_body);
system.Add(rotation_link);

system.Set_G_acc({0.0, 0.0, 10.0});

for (double time{}; time < 1.0; time += 0.01)
{
    system.DoFrameDynamics(time);

    std::cout << std::fixed << std::setprecision(3) << "Time: " << system.GetChTime()
              << " *** Force: " << rotation_link->Get_react_force()

              << " *** Torque: " << rotation_link->Get_react_torque()
              << " *** Link Absolute Coords: " << rotation_link->GetLinkAbsoluteCoords() << "\n";
}

Output:

...

Time: 0.830 *** Force: 0.682  0.731  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.840 *** Force: 0.675  0.738  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.850 *** Force: 0.667  0.745  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.860 *** Force: 0.660  0.751  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.870 *** Force: 0.652  0.758  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.880 *** Force: 0.645  0.764  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.890 *** Force: 0.637  0.771  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.900 *** Force: 0.629  0.777  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.910 *** Force: 0.622  0.783  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.920 *** Force: 0.614  0.790  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.930 *** Force: 0.606  0.796  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.940 *** Force: 0.598  0.802  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.950 *** Force: 0.590  0.808  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.960 *** Force: 0.582  0.813  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.970 *** Force: 0.574  0.819  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.980 *** Force: 0.565  0.825  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.990 *** Force: 0.557  0.831  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000

[Luning Fang]

The object is traveling in a circular motion at a constant angular velocity, 1, with respect to the global z coordinate, gravity pointing downward z. Therefore, link force in xy plane is a centrifugal force, F = mw^2R, pointing towards origin, z component balance out gravitational force. With zero angular acceleration, z-component of link torque is zero, y-component balance out the torque from gravity, since rotation around y and z axis are constrained.

Thank you,

Luning

[Simon]

Hi Luning,

thanks for your reply. I agree on the cause of the force and torque components, but to my understanding the constant torque vector [0.000, -10.000, -0.000] would be correct only in the rotating frame, not in the link frame (which is fixed here).

I modified my example, now using ChLinkLockLock to create the same rotating body setup (see below). The torque vector seems to rotate correctly when using ChLinkLockLock, in contrast to the ChLinkMotorRotationSpeed case.

Thank you,
Simon

Code:

ChSystemNSC system{};

auto fixed_body{chrono_types::make_shared<ChBody>()};
fixed_body->SetBodyFixed(true);
auto rotating_body{chrono_types::make_shared<ChBody>()};
rotating_body->SetPos({1.0, 0.0, 0.0});

// Option 1: ChLinkMotorRotationSpeed
//        auto rotation_link{chrono_types::make_shared<ChLinkMotorRotationSpeed>()};
//        rotation_link->Initialize(rotating_body, fixed_body, ChFrame{});
//        rotation_link->SetSpeedFunction(chrono_types::make_shared<ChFunction_Const>(1.0));

// Option 2: ChLinkLockLock
auto rotation_link{chrono_types::make_shared<ChLinkLockLock>()};
rotation_link->Initialize(rotating_body, fixed_body, {});
rotation_link->Set_angleset(chrono::AngleSet::ANGLE_AXIS);
rotation_link->SetMotion_axis({0.0, 0.0, 1.0});
rotation_link->SetMotion_ang(chrono_types::make_shared<ChFunction_Ramp>(0.0, 1.0));

system.Add(fixed_body);
system.Add(rotating_body);
system.Add(rotation_link);

system.Set_G_acc({0.0, 0.0, 10.0});

for (double time{}; time < 1.0; time += 0.01)
{
    system.DoFrameDynamics(time);

    std::cout << std::fixed << std::setprecision(3) << "Time: " << system.GetChTime()
              << " *** Force: " << rotation_link->Get_react_force()
              << " *** Torque: " << rotation_link->Get_react_torque()
              << " *** Link Absolute Coords: " << rotation_link->GetLinkAbsoluteCoords() << "\n";
}

Output using ChLinkMotorRotationSpeed:
...

Time: 0.970 *** Force: 0.574  0.819  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.980 *** Force: 0.565  0.825  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.990 *** Force: 0.557  0.831  10.000 *** Torque: 0.000  -10.000  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000

Output using ChLinkLockLock:

...

Time: 0.970 *** Force: 0.574  0.819  10.000 *** Torque: 4.662  -8.847  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.980 *** Force: 0.565  0.825  10.000 *** Torque: 4.707  -8.824  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000
Time: 0.990 *** Force: 0.557  0.831  10.000 *** Torque: 4.751  -8.800  -0.000 *** Link Absolute Coords: 0.000  0.000  0.000  1.000  0.000  0.000  0.000

[chao peng]

Dear everyone,

We corrected the bugs reported here.

Please refer to the commit [d7193ef] on 17th April.

Welcome to feedback your test result.

[Simon]

Hi,

thank you for the updated implementation. I re-ran my test case with chrono at commit 841781d6e0e4b09c68cd431e1f7381b402f17ad1.

The reaction torque vector does now rotate as expected with ChLinkMotorRotationSpeed. As shown in my example below there is now a constant angle difference of 90° between force and torque in the x-y plane.

However, with the ChLinkLockLock implementation I noticed that the angle of the torque vector seems to change only at half the speed of the body rotation. This leads to a constantly changing difference between force and torque direction in the x-y plane.
So perhaps there is also something wrong with the ChLinkLockLock's reaction torque direction (see example with output below).

Sorry for being naggy, your support is highly appreciated!

Best regards,
Simon

Code:

ChSystemNSC system{};

auto fixed_body{chrono_types::make_shared<ChBody>()};
fixed_body->SetBodyFixed(true);
auto rotating_body{chrono_types::make_shared<ChBody>()};
rotating_body->SetPos({1.0, 0.0, 0.0});

// Option 1: ChLinkMotorRotationSpeed

auto rotation_link{chrono_types::make_shared<ChLinkMotorRotationSpeed>()};
rotation_link->Initialize(rotating_body, fixed_body, ChFrame{});
rotation_link->SetSpeedFunction(chrono_types::make_shared<ChFunction_Const>(1.0));

// Option 2: ChLinkLockLock
//        auto rotation_link{chrono_types::make_shared<ChLinkLockLock>()};
//        rotation_link->Initialize(rotating_body, fixed_body, {});
//        rotation_link->Set_angleset(chrono::AngleSet::ANGLE_AXIS);
//        rotation_link->SetMotion_axis({0.0, 0.0, 1.0});
//        rotation_link->SetMotion_ang(chrono_types::make_shared<ChFunction_Ramp>(0.0, 1.0));

system.Add(fixed_body);
system.Add(rotating_body);
system.Add(rotation_link);

system.Set_G_acc({0.0, 0.0, 10.0});

for (double time{}; time < 1.0; time += 0.01)
{
    system.DoFrameDynamics(time);

    const auto body_position{rotating_body->GetPos()};
    const double body_angle{std::atan2(body_position[1], body_position[0])};

    const auto reaction_force{rotation_link->Get_react_force()};
    const double force_angle{std::atan2(reaction_force[1], reaction_force[0])};

    const auto reaction_torque{rotation_link->Get_react_torque()};
    const double torque_angle{std::atan2(reaction_torque[1], reaction_torque[0])};

    std::cout << std::fixed << std::setprecision(3) << "Time: " << system.GetChTime()

              << " *** Body Angle: " << body_angle << " *** Force Angle: " << force_angle
              << " *** Torque Angle: " << torque_angle
              << " *** Angle Difference: " << force_angle - torque_angle << "\n";
}

Output using ChLinkMotorRotationSpeed:

...

Time: 0.950 *** Body Angle: 0.950 *** Force Angle: 0.940 *** Torque Angle: -0.631 *** Angle Difference: 1.571
Time: 0.960 *** Body Angle: 0.960 *** Force Angle: 0.950 *** Torque Angle: -0.621 *** Angle Difference: 1.571
Time: 0.970 *** Body Angle: 0.970 *** Force Angle: 0.960 *** Torque Angle: -0.611 *** Angle Difference: 1.571
Time: 0.980 *** Body Angle: 0.980 *** Force Angle: 0.970 *** Torque Angle: -0.601 *** Angle Difference: 1.571
Time: 0.990 *** Body Angle: 0.990 *** Force Angle: 0.980 *** Torque Angle: -0.591 *** Angle Difference: 1.571

Output using ChLinkLockLock:

...

Time: 0.950 *** Body Angle: 0.950 *** Force Angle: 0.940 *** Torque Angle: -1.096 *** Angle Difference: 2.036
Time: 0.960 *** Body Angle: 0.960 *** Force Angle: 0.950 *** Torque Angle: -1.091 *** Angle Difference: 2.041
Time: 0.970 *** Body Angle: 0.970 *** Force Angle: 0.960 *** Torque Angle: -1.086 *** Angle Difference: 2.046
Time: 0.980 *** Body Angle: 0.980 *** Force Angle: 0.970 *** Torque Angle: -1.081 *** Angle Difference: 2.051
Time: 0.990 *** Body Angle: 0.990 *** Force Angle: 0.980 *** Torque Angle: -1.076 *** Angle Difference: 2.056

[chao peng]

Hello Simon,

Thank you for your test and feedback. I didn't realize there is a same bug as in ChLinkMateGeneric().

When we use quaternion to parameterize the finite rotation and derive the constraint equation (C) and its Jacobian matrix (Cq), we will have a projection matrix P to convert the Lagrange multiplier (only for rotational DOFs) to the reaction torque. This projection matrix P = 0.5(s*I33+\tilde(v)) where \pho_{F1_F2}=(s,v) is the quaternion of the slave frame F1 to master frame F2. When F1, F2 are coincident, P=0.5*I33, thus we can find a coefficient 0.5 in the code to convert Lagrange multipler to reaction torque. But in case there is a relative rotation between F1 and F2, the projection matrix P changes, and the Lagrange multiplier is expressed in an obscure ghost frame FX (let's denote it as FX here). The rotation of FX is a half of the relative rotation in \pho_{F1_F2}. Besides this point, it also includes a component which scales the Lagrange multiplier by the scalar part 's'. This is why you obtain the above result in you test.

We fixed this bug in below commit:

Commit: fb7d16caa653d837c9220cf96db5aebeb2df8231 [fb7d16c], on 27th April.

Try to pull it and test again.

I would like to remind here, the tangent stiffness matrix (Kc) of ChLinkLockLock() is not implemented, although the analytical expression should be same as ChLinkMateGeneric(), or at least quite similar. Thus you cannot perform the static and eigenvalue analysis for a pendulum using ChLinkLockLock().

Best regards,

Chao PENG.

[chao peng]

Dear users,

The explanation for the bug and the previous commit ([fb7d16c], on 27th April) are wrong. The bug of ChLinkLockLock() is not related to the projection matrix P.

Today I pushed another commit to fix the bug:

Commit: ca81537edc33c128af3dbfb820b9039aac42e00f [ca81537]

The reaction torque is simply computed as Cqw2.T*lambda which is expressed in the local frame of Body2, then it is rotated back to the master frame (marker2, or frame2, F2) of the joint by left-multiplying R_F2_B2.T.

Best regards,

Chao PENG.

[Simon]

Hi,

with commit ca81537edc33c128af3dbfb820b9039aac42e00f I obtained the results below. The slight differences between the implementations could perhaps be avoided with different solver settings.
It seems that the corrected implementation also resolves this issue: https://groups.google.com/g/projectchrono/c/m2tswtNze2Q

Thank you very much!
Simon

Output using ChLinkMotorRotationSpeed:

...

Time: 0.950 *** Body Angle: 0.950 *** Force Angle: 0.940 *** Torque Angle: -0.631 *** Angle Difference: 1.571
Time: 0.960 *** Body Angle: 0.960 *** Force Angle: 0.950 *** Torque Angle: -0.621 *** Angle Difference: 1.571
Time: 0.970 *** Body Angle: 0.970 *** Force Angle: 0.960 *** Torque Angle: -0.611 *** Angle Difference: 1.571
Time: 0.980 *** Body Angle: 0.980 *** Force Angle: 0.970 *** Torque Angle: -0.601 *** Angle Difference: 1.571
Time: 0.990 *** Body Angle: 0.990 *** Force Angle: 0.980 *** Torque Angle: -0.591 *** Angle Difference: 1.571

Output using ChLinkLockLock:

...

Time: 0.950 *** Body Angle: 0.950 *** Force Angle: 0.940 *** Torque Angle: -0.621 *** Angle Difference: 1.561
Time: 0.960 *** Body Angle: 0.960 *** Force Angle: 0.950 *** Torque Angle: -0.611 *** Angle Difference: 1.561
Time: 0.970 *** Body Angle: 0.970 *** Force Angle: 0.960 *** Torque Angle: -0.601 *** Angle Difference: 1.561
Time: 0.980 *** Body Angle: 0.980 *** Force Angle: 0.970 *** Torque Angle: -0.591 *** Angle Difference: 1.561
Time: 0.990 *** Body Angle: 0.990 *** Force Angle: 0.980 *** Torque Angle: -0.581 *** Angle Difference: 1.561