search

epfl-lasa / control-libraries

A collection of library modules to facilitate the creation of full control loop algorithms, including state representation, motion planning, kinematics, dynamics and control.
https://epfl-lasa.github.io/control-libraries
GNU General Public License v3.0
27 stars 2 forks source link

Throw exception if setting state variable from vector with wrong size #273

Closed domire8 closed 2 years ago

domire8 commented 2 years ago

Soo, funny thing that I came across when I saw a script that dd something along the following:

x, xd, xdd = generate_minimum_jerk(start.get_position(), goal.get_position())
# x is a list of a list
state = sr,CartesianState("test")
state.set_position(x[i]) # but wait, x[i] has a loooot of elements, why does it not fail???
state.set_linear_velocity(xd[i]) # that fails

Printing the state after setting positions:

Invoked with: test CartesianState expressed in world frame
position: (0.59688, 0.596875, 0.596841)
orientation: (1, 0, 0, 0) <=> theta: 0, axis: (1, 0, 0)
linear velocity: (0, 0, 0)
angular velocity: (0, 0, 0)
linear acceleration: (0, 0, 0)
angular acceleration: (0, 0, 0)
force: (0, 0, 0)
torque: (0, 0, 0), array([ 0.00000000e+00, -1.48133451e-03, -5.80502972e-03, -1.27933995e-02,
       -2.22724597e-02, -3.40719276e-02, -4.80252225e-02, -6.39694656e-02,
       -8.17454797e-02, -1.01197789e-01, -1.22174621e-01, -1.44527903e-01,
       -1.68113266e-01, -1.92790041e-01, -2.18421261e-01, -2.44873663e-01,
       -2.72017684e-01, -2.99727461e-01, -3.27880838e-01, -3.56359355e-01,
       -3.85048257e-01, -4.13836491e-01, -4.42616704e-01, -4.71285247e-01,
       -4.99742170e-01, -5.27891227e-01, -5.55639874e-01, -5.82899267e-01,
       -6.09584265e-01, -6.35613428e-01, -6.60909020e-01, -6.85397003e-01,
       -7.09007044e-01, -7.31672511e-01, -7.53330473e-01, -7.73921701e-01,
       -7.93390669e-01, -8.11685551e-01, -8.28758224e-01, -8.44564267e-01,
       -8.59062960e-01, -8.72217284e-01, -8.83993924e-01, -8.94363266e-01,
       -9.03299396e-01, -9.10780104e-01, -9.16786882e-01, -9.21304921e-01,
       -9.24323117e-01, -9.25834066e-01, -9.25834066e-01, -9.24323117e-01,
       -9.21304921e-01, -9.16786882e-01, -9.10780104e-01, -9.03299396e-01,
       -8.94363266e-01, -8.83993924e-01, -8.72217284e-01, -8.59062960e-01,
       -8.44564267e-01, -8.28758224e-01, -8.11685551e-01, -7.93390669e-01,
       -7.73921701e-01, -7.53330473e-01, -7.31672511e-01, -7.09007044e-01,
       -6.85397003e-01, -6.60909020e-01, -6.35613428e-01, -6.09584265e-01,
       -5.82899267e-01, -5.55639874e-01, -5.27891227e-01, -4.99742170e-01,
       -4.71285247e-01, -4.42616704e-01, -4.13836491e-01, -3.85048257e-01,
       -3.56359355e-01, -3.27880838e-01, -2.99727461e-01, -2.72017684e-01,
       -2.44873663e-01, -2.18421261e-01, -1.92790041e-01, -1.68113266e-01,
       -1.44527903e-01, -1.22174621e-01, -1.01197789e-01, -8.17454797e-02,
       -6.39694656e-02, -4.80252225e-02, -3.40719276e-02, -2.22724597e-02,
       -1.27933995e-02, -5.80502972e-03, -1.48133451e-03,  3.03576608e-18,
        3.03576608e-18])

So somehow, the positions are set with the first 3 elements of x[i] and the rest is appended in an array.

All that to say we need to check for the size of the vector/list if we set state variables in the CartesianState. In cpp it would give an Eigen runtime error, but we wanna catch that ourselves.