search

qiayuanl / legged_control

Nonlinear MPC and WBC framework for legged robot based on OCS2 and ros-controls
BSD 3-Clause "New" or "Revised" License
854 stars 217 forks source link

SOLVED - robot wants to standup, but motors are trembling a lot... #32

Closed elpimous closed 1 year ago

elpimous commented 1 year ago

Hey all, my robot is Ylo2, imu : Myahrs+ ubuntu 20.04 not RT kernel (for now) controllers (Peak 4 can M2)

1 / udrf test model + controllers feedback : https://youtu.be/R_Hy4YlnHUo 2 / test standup with lot of tremblings (needed to cut torque with a security button) : https://youtu.be/dH9bFtzb-CI

Here is the rqt_tf_tree : frames

Regarding to imu, it feeds correctly UnitreeHW.cpp, under sensor_msgs imu_message my xacro file use this imu link name : 

<joint name="imu_joint" type="fixed">
        <parent link="base"/>
        <child link="imu_link"/>
        <origin rpy="0 0 0" xyz="0 0 0"/>
    </joint>

    <link name="imu_link">
   ....

<xacro:IMU connected_to="base" imu_name="unitree_imu" xyz="0. 0. 0." rpy="0. 0. 0."/> Perhaps a problem here ?

and regarding to the tremblings, can I reduce kp and kd values, Where ?

Impatient to read you.

Vincent

elpimous commented 1 year ago

here is the verbose activated : 

robot is : a1
[LeggedInterface] Loading task file: "/home/ylo2/catkin_ws/src/legged_control/legged_controllers/config/a1/task.info"
[LeggedInterface] Loading Pinocchio model from: "/tmp/legged_control/a1.urdf"
[LeggedInterface] Loading target command settings from: "/home/ylo2/catkin_ws/src/legged_control/legged_controllers/config/a1/reference.info"

 #### Legged Robot Model Settings:
 #### =============================================================================
 #### 'positionErrorGain'......................................................0
 #### 'phaseTransitionStanceTime'............................................0.1
 #### 'verboseCppAd'...........................................................1
 #### 'recompileLibrariesCppAd'................................................0
 #### 'modelFolderCppAd'................................../tmp/legged_control/a1
 #### =============================================================================

 #### MPC Settings:
 #### =============================================================================
 #### 'timeHorizon'............................................................1
 #### 'solutionTimeWindow'....................................................-1
 #### 'coldStart'..............................................................0
 #### 'debugPrint'.............................................................0
 #### 'mpcDesiredFrequency'..................................................100
 #### 'mrtDesiredFrequency'.................................................1000
 #### =============================================================================

 #### DDP Settings: 
 #### =============================================================================
 #### 'algorithm'............................................................SLQ
 #### 'nThreads'...............................................................3
 #### 'threadPriority'........................................................50
 #### 'maxNumIterations'.......................................................1
 #### 'minRelCost'...........................................................0.1
 #### 'constraintTolerance'................................................0.005
 #### 'displayInfo'............................................................1
 #### 'displayShortSummary'....................................................0
 #### 'checkNumericalStability'................................................0
 #### 'debugPrintRollout'......................................................1
 #### 'AbsTolODE'..........................................................1e-05
 #### 'RelTolODE'..........................................................0.001
 #### 'maxNumStepsPerSecond'...............................................10000
 #### 'timeStep'...........................................................0.015
 #### 'backwardPassIntegratorType'.........................................ODE45
 #### 'constraintPenaltyInitialValue'.........................................20
 #### 'constraintPenaltyIncreaseRate'..........................................2
 #### 'preComputeRiccatiTerms'.................................................1
 #### 'useFeedbackPolicy'......................................................0
 #### 'riskSensitiveCoeff'.....................................................0 (default)
 #### 'strategy'.....................................................LINE_SEARCH
 #### LINE_SEARCH Settings: {
 #### 'minStepLength'.......................................................0.01
 #### 'maxStepLength'..........................................................1
 #### 'contractionRate'......................................................0.5 (default)
 #### 'armijoCoefficient'.................................................0.0001 (default)
 #### 'hessianCorrectionStrategy'.................................DIAGONAL_SHIFT
 #### 'hessianCorrectionMultiple'..........................................1e-05
 #### }
 #### =============================================================================

 #### Multiple-Shooting SQP Settings:
 #### =============================================================================
 #### 'sqpIteration'...........................................................1
 #### 'deltaTol'..........................................................0.0001
 #### 'alpha_decay'..........................................................0.5 (default)
 #### 'alpha_min'.........................................................0.0001 (default)
 #### 'gamma_c'............................................................1e-06 (default)
 #### 'g_max'...............................................................0.01
 #### 'g_min'..............................................................1e-06
 #### 'armijoFactor'......................................................0.0001 (default)
 #### 'costTol'...........................................................0.0001 (default)
 #### 'dt'.................................................................0.015
 #### 'useFeedbackPolicy'......................................................0
 #### 'createValueFunction'....................................................0 (default)
 #### 'integratorType'.......................................................RK2
 #### 'inequalityConstraintMu'...............................................0.1
 #### 'inequalityConstraintDelta'..............................................5
 #### 'projectStateInputEqualityConstraints'...................................1
 #### 'extractProjectionMultiplier'............................................0 (default)
 #### 'printSolverStatus'......................................................0
 #### 'printSolverStatistics'..................................................1
 #### 'printLinesearch'........................................................0
 #### 'nThreads'...............................................................3
 #### 'threadPriority'........................................................50

 #### HPIPM Settings:
 #### =============================================================================
 #### 'mode'...................................................................1 (default)
 #### 'iter_max'..............................................................30 (default)
 #### 'alpha_min'..........................................................1e-12 (default)
 #### 'mu0'...................................................................10 (default)
 #### 'tol_stat'...........................................................1e-06 (default)
 #### 'tol_eq'.............................................................1e-08 (default)
 #### 'tol_ineq'...........................................................1e-08 (default)
 #### 'tol_comp'...........................................................1e-08 (default)
 #### 'reg_prim'...........................................................1e-12 (default)
 #### 'warm_start'.............................................................0 (default)
 #### 'pred_corr'..............................................................1 (default)
 #### 'ric_alg'................................................................0 (default)
 #### =============================================================================
 #### =============================================================================

 #### Multiple-Shooting IPM Settings:
 #### =============================================================================
 #### 'ipmIteration'...........................................................1
 #### 'deltaTol'..........................................................0.0001
 #### 'alpha_decay'..........................................................0.5 (default)
 #### 'alpha_min'.........................................................0.0001 (default)
 #### 'gamma_c'............................................................1e-06 (default)
 #### 'g_max'.................................................................10
 #### 'g_min'..............................................................1e-06
 #### 'armijoFactor'......................................................0.0001 (default)
 #### 'costTol'...........................................................0.0001 (default)
 #### 'dt'.................................................................0.015
 #### 'useFeedbackPolicy'......................................................0
 #### 'createValueFunction'....................................................0 (default)
 #### 'computeLagrangeMultipliers'.............................................1
 #### 'integratorType'.......................................................RK2
 #### 'initialBarrierParameter'...........................................0.0001
 #### 'targetBarrierParameter'............................................0.0001
 #### 'barrierReductionCostTol '............................................0.01 (default)
 #### 'barrierReductionConstraintTol'......................................0.001
 #### 'barrierLinearDecreaseFactor'..........................................0.2
 #### 'barrierSuperlinearDecreasePower'......................................1.5
 #### 'fractionToBoundaryMargin'...........................................0.995
 #### 'usePrimalStepSizeForDual'...............................................0
 #### 'initialSlackLowerBound'............................................0.0001
 #### 'initialDualLowerBound'.............................................0.0001
 #### 'initialSlackMarginRate'..............................................0.01
 #### 'initialDualMarginRate'...............................................0.01
 #### 'printSolverStatus'......................................................0
 #### 'printSolverStatistics'..................................................1
 #### 'printLinesearch'........................................................0
 #### 'nThreads'...............................................................3
 #### 'threadPriority'........................................................50

 #### HPIPM Settings:
 #### =============================================================================
 #### 'mode'...................................................................1 (default)
 #### 'iter_max'..............................................................30 (default)
 #### 'alpha_min'..........................................................1e-12 (default)
 #### 'mu0'...................................................................10 (default)
 #### 'tol_stat'...........................................................1e-06 (default)
 #### 'tol_eq'.............................................................1e-08 (default)
 #### 'tol_ineq'...........................................................1e-08 (default)
 #### 'tol_comp'...........................................................1e-08 (default)
 #### 'reg_prim'...........................................................1e-12 (default)
 #### 'warm_start'.............................................................0 (default)
 #### 'pred_corr'..............................................................1 (default)
 #### 'ric_alg'................................................................0 (default)
 #### =============================================================================
 #### =============================================================================

 #### Rollout Settings: 
 #### =============================================================================
 #### 'AbsTolODE'..........................................................1e-05
 #### 'RelTolODE'..........................................................0.001
 #### 'maxNumStepsPerSecond'...............................................10000
 #### 'timeStep'...........................................................0.015
 #### 'integratorType'.....................................................ODE45
 #### 'checkNumericalStability'................................................0
 #### 'reconstructInputTrajectory'.............................................1 (default)
 #### 'rootFindingAlgorithm'...................................................0 (default)
 #### 'maxSingleEventIterations'..............................................10 (default)
 #### 'useTrajectorySpreadingController'.......................................0 (default)
 #### =============================================================================

 #### Swing Trajectory Config:
 #### =============================================================================
 #### 'liftOffVelocity'.....................................................0.05
 #### 'touchDownVelocity'...................................................-0.1
 #### 'swingHeight'.........................................................0.08
 #### 'swingTimeScale'......................................................0.15
 #### =============================================================================

#### Modes Schedule: 
#### =============================================================================
Initial Modes Schedule: 
event times:   {0.5}
mode sequence: {15, 15}
Default Modes Sequence Template: 
Template switching times: {0, 1}
Template mode sequence:   {15}
#### =============================================================================
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_lib.so
WARNING: Loaded at least one default value in matrix: "Q"
WARNING: Loaded at least one default value in matrix: "R"

 #### Base Tracking Cost Coefficients: 
 #### =============================================================================
Q:
  15    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
   0   15    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
   0    0  100    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
   0    0    0   10    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
   0    0    0    0   30    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
   0    0    0    0    0   30    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
   0    0    0    0    0    0 1000    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
   0    0    0    0    0    0    0 1000    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
   0    0    0    0    0    0    0    0 1500    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0
   0    0    0    0    0    0    0    0    0  100    0    0    0    0    0    0    0    0    0    0    0    0    0    0
   0    0    0    0    0    0    0    0    0    0  300    0    0    0    0    0    0    0    0    0    0    0    0    0
   0    0    0    0    0    0    0    0    0    0    0  300    0    0    0    0    0    0    0    0    0    0    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    5    0    0    0    0    0    0    0    0    0    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    5    0    0    0    0    0    0    0    0    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    0  2.5    0    0    0    0    0    0    0    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    5    0    0    0    0    0    0    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    5    0    0    0    0    0    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  2.5    0    0    0    0    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    5    0    0    0    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    5    0    0    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  2.5    0    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    5    0    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    5    0
   0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  2.5
R:
     0.001          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0
         0      0.001          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0
         0          0      0.001          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0
         0          0          0      0.001          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0
         0          0          0          0      0.001          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0
         0          0          0          0          0      0.001          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0
         0          0          0          0          0          0      0.001          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0
         0          0          0          0          0          0          0      0.001          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0
         0          0          0          0          0          0          0          0      0.001          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0
         0          0          0          0          0          0          0          0          0      0.001          0          0          0          0          0          0          0          0          0          0          0          0          0          0
         0          0          0          0          0          0          0          0          0          0      0.001          0          0          0          0          0          0          0          0          0          0          0          0          0
         0          0          0          0          0          0          0          0          0          0          0      0.001          0          0          0          0          0          0          0          0          0          0          0          0
         0          0          0          0          0          0          0          0          0          0          0          0   0.597022 -0.0163812 -0.0607101          0          0          0          0          0          0          0          0          0
         0          0          0          0          0          0          0          0          0          0          0          0 -0.0163812   0.557844  -0.295607          0          0          0          0          0          0          0          0          0
         0          0          0          0          0          0          0          0          0          0          0          0 -0.0607101  -0.295607     0.2645          0          0          0          0          0          0          0          0          0
         0          0          0          0          0          0          0          0          0          0          0          0          0          0          0   0.597022 -0.0163812 -0.0607101          0          0          0          0          0          0
         0          0          0          0          0          0          0          0          0          0          0          0          0          0          0 -0.0163812   0.557844  -0.295607          0          0          0          0          0          0
         0          0          0          0          0          0          0          0          0          0          0          0          0          0          0 -0.0607101  -0.295607     0.2645          0          0          0          0          0          0
         0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0   0.597022  0.0163812  0.0607101          0          0          0
         0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0  0.0163812   0.557844  -0.295607          0          0          0
         0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0  0.0607101  -0.295607     0.2645          0          0          0
         0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0   0.597022  0.0163812  0.0607101
         0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0  0.0163812   0.557844  -0.295607
         0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0          0  0.0607101  -0.295607     0.2645
 #### =============================================================================

 #### Friction Cone Settings: 
 #### =============================================================================
 #### 'frictionCoefficient'..................................................0.3
 #### 'mu'...................................................................0.1
 #### 'delta'..................................................................5
 #### =============================================================================
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/LF_FOOT_position/cppad_generated/LF_FOOT_position_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/LF_FOOT_velocity/cppad_generated/LF_FOOT_velocity_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/LF_FOOT_orientation/cppad_generated/LF_FOOT_orientation_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/RF_FOOT_position/cppad_generated/RF_FOOT_position_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/RF_FOOT_velocity/cppad_generated/RF_FOOT_velocity_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/RF_FOOT_orientation/cppad_generated/RF_FOOT_orientation_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/LH_FOOT_position/cppad_generated/LH_FOOT_position_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/LH_FOOT_velocity/cppad_generated/LH_FOOT_velocity_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/LH_FOOT_orientation/cppad_generated/LH_FOOT_orientation_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/RH_FOOT_position/cppad_generated/RH_FOOT_position_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/RH_FOOT_velocity/cppad_generated/RH_FOOT_velocity_lib.so
[CppAdInterface] Loading Shared Library: /tmp/legged_control/a1/RH_FOOT_orientation/cppad_generated/RH_FOOT_orientation_lib.so

 #### SelfCollision Settings: 
 #### =============================================================================
 #### 'mu'..................................................................0.01
 #### 'delta'..............................................................0.001
 #### 'minimumDistance'.....................................................0.05
 #### 'selfCollision.collisionObjectPairs': { }
 #### 'selfCollision.collisionLinkPairs': {[LF_calf, RF_calf], [LH_calf, RH_calf], [LF_calf, LH_calf], [RF_calf, RH_calf], [LF_FOOT, RF_FOOT], [LH_FOOT, RH_FOOT], [LF_FOOT, LH_FOOT], [RF_FOOT, RH_FOOT]} 
 #### =============================================================================
SelfCollision: Testing for 8 collision pairs

 #### Kalman Filter Noise:
 #### =============================================================================
 #### 'footRadius'..........................................................0.02
 #### 'imuProcessNoisePosition'.............................................0.02
 #### 'imuProcessNoiseVelocity'.............................................0.02
 #### 'footProcessNoisePosition'...........................................0.002
 #### 'footSensorNoisePosition'............................................0.005
 #### 'footSensorNoiseVelocity'..............................................0.1
 #### 'footHeightSensorNoise'...............................................0.01

 #### Torque Limits Task:
 #### =============================================================================

 #### HAA HFE KFE: 33.5 33.5 33.5
 #### =============================================================================

 #### Friction Cone Task:
 #### =============================================================================
 #### 'frictionCoefficient'..................................................0.3
 #### =============================================================================

 #### Swing Leg Task:
 #### =============================================================================
 #### 'kp'...................................................................3.5
 #### 'kd'...................................................................0.1

 #### WBC weight:
 #### =============================================================================
 #### 'swingLeg'.............................................................100
 #### 'baseAccel'..............................................................1
 #### 'contactForce'........................................................0.01
[ WARN] [1681124071.374601778]: Cycle time exceeded error threshold by: 0.000148526s, cycle time: 0.008481859s, threshold: 0.005s

[ WARN] [1681124093.800020524]: Controller Manager: To switch controllers you need to specify a strictness level of controller_manager_msgs::SwitchController::STRICT (2) or ::BEST_EFFORT (1). Defaulting to ::BEST_EFFORT.
-0.0298627
0.00157184
0.260617
0.964979
[ INFO] [1681124093.803979499]: Waiting for the initial policy ...
[ INFO] [1681124093.827376024]: Initial policy has been received.
...
########################################################################
### MPC Benchmarking
###   Maximum : 89.5455[ms].
###   Average : 17.1181[ms].
########################################################################
### WBC Benchmarking
###   Maximum : 4.89206[ms].
###   Average : 0.865205[ms].
########################################################################
The benchmarking is computed over 394 iterations. 
SQP Benchmarking       :    Average time [ms]   (% of total runtime)
    LQ Approximation   :    13.7896 [ms]        (81.1685%)
    Solve QP           :    1.59765 [ms]        (9.40408%)
    Linesearch         :    1.53532 [ms]        (9.03719%)
    Compute Controller :    0.0663046 [ms]      (0.390283%)
elpimous commented 1 year ago

Here is the optimizedStateTrajectory topic

...
   header: 
      seq: 0
      stamp: 
        secs: 1681132283
        nsecs:  64794388
      frame_id: "odom"
    ns: "EE Trajectories"
    id: 3
    type: 4
    action: 0
    pose: 
      position: 
        x: 0.0
        y: 0.0
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
    scale: 
      x: 0.01
      y: 0.0
      z: 0.0
    color: 
      r: 0.49399998784065247
      g: 0.18400000035762787
      b: 0.5559999942779541
      a: 1.0
    lifetime: 
      secs: 0
      nsecs:         0
    frame_locked: False
    points: 
      - 
        x: -0.07282175907041782
        y: -0.21744574327779392
        z: 0.04189845999517791
      - 
        x: -0.07333772383922296
        y: -0.21787074283876753
        z: 0.044914922427384464
      - 
        x: -0.07369891986890906
        y: -0.21760280109067098
        z: 0.0458083522281444
      - 
        x: -0.07400726752713355
        y: -0.21734715639868368
        z: 0.04616821264391105
      - 
        x: -0.07424753976338058
        y: -0.21713859438761915
        z: 0.04627553238143201
      - 
        x: -0.07442594981346476
        y: -0.21696189712024316
        z: 0.04628452952381518
      - 
        x: -0.07445448814823266
        y: -0.21693407464229442
        z: 0.04612742993527127
      - 
        x: -0.07445448814823266
        y: -0.21693407464229442
        z: 0.04612742993527127
      - 
        x: -0.07451996778669209
        y: -0.2168512154018985
        z: 0.046010002220845954
      - 
        x: -0.0745432429717926
        y: -0.21681501398400874
        z: 0.0458607306714615
      - 
        x: -0.07454417800245922
        y: -0.2167645820636443
        z: 0.04570276484223951
      - 
        x: -0.07453102366897468
        y: -0.21673188406098254
        z: 0.04554214741129925
      - 
        x: -0.07450665182735616
        y: -0.21671442882278463
        z: 0.04538560300787984
      - 
        x: -0.07447381845694137
        y: -0.2167101221126999
        z: 0.045236231219288686
      - 
        x: -0.07443461587142536
        y: -0.21671714700824363
        z: 0.045095448593246865
      - 
        x: -0.07439047468784116
        y: -0.21673391303079506
        z: 0.04496380072005341
      - 
        x: -0.07434227882591377
        y: -0.21675902898502974
        z: 0.04484136096120353
      - 
        x: -0.07429048755297263
        y: -0.21679127705900342
        z: 0.04472793994569413
      - 
        x: -0.07423524149424626
        y: -0.21682958685728884
        z: 0.04462320357246845
      - 
        x: -0.07417645340482545
        y: -0.21687300918670183
        z: 0.044526740929420744
      - 
        x: -0.07411389153838882
        y: -0.2169206900304596
        z: 0.04443810312152699
      - 
        x: -0.0740472664951484
        y: -0.216971842784682
        z: 0.04435682138545281
      - 
        x: -0.07397633354608768
        y: -0.21702571480380534
        z: 0.04428240687342301
      - 
        x: -0.07390103107032622
        y: -0.21708154158556106
        z: 0.04421432583653043
      - 
        x: -0.0738216887696321
        y: -0.21713847693880395
        z: 0.04415192929048133
      - 
        x: -0.07373937699025632
        y: -0.2171954789323055
        z: 0.04409427939128479
      - 
        x: -0.07365657150203883
        y: -0.21725117496009724
        z: 0.044039787989323884
      - 
        x: -0.07357588858903427
        y: -0.2173042837947209
        z: 0.043986990490871694
      - 
        x: -0.07349905952595454
        y: -0.21735402954307828
        z: 0.043935177810988546
      - 
        x: -0.07342706649745043
        y: -0.21740006988214614
        z: 0.04388417583170097
      - 
        x: -0.07336042621193029
        y: -0.21744232254544385
        z: 0.04383405182225884
      - 
        x: -0.07329935189682363
        y: -0.2174808571194342
        z: 0.04378496990299775
      - 
        x: -0.07324385426988494
        y: -0.21751583034924823
        z: 0.043737119861066875
      - 
        x: -0.07319380783702362
        y: -0.21754744644576587
        z: 0.04369068143510578
      - 
        x: -0.07314899625830582
        y: -0.2175759328676753
        z: 0.043645807809739715
      - 
        x: -0.07310914440701533
        y: -0.21760152513498668
        z: 0.0436026188160918
      - 
        x: -0.07307394174268536
        y: -0.21762445738212188
        z: 0.04356119943848198
      - 
        x: -0.07304305779276268
        y: -0.21764495694891817
        z: 0.04352160239491927
      - 
        x: -0.07301615500474931
        y: -0.21766324090626843
        z: 0.043483851208464167
      - 
        x: -0.07299289814858068
        y: -0.21767951412685407
        z: 0.043447943945815845
      - 
        x: -0.07297296114473079
        y: -0.2176939683281527
        z: 0.04341385714294907
      - 
        x: -0.07295603203046694
        y: -0.21770678177942743
        z: 0.04338154960583421
      - 
        x: -0.07294181649999526
        y: -0.21771811944576194
        z: 0.04335096590021961
      - 
        x: -0.07293004026259295
        y: -0.2177281333837499
        z: 0.04332203929785226
      - 
        x: -0.07292045058054519
        y: -0.2177369633629307
        z: 0.04329469438308729
      - 
        x: -0.07291281711532469
        y: -0.21774473760686558
        z: 0.043268849172664964
      - 
        x: -0.07290693225307601
        y: -0.21775157361777164
        z: 0.04324441678094176
      - 
        x: -0.07290261104311224
        y: -0.21775757905685952
        z: 0.04322130666126818
      - 
        x: -0.07289969085604961
        y: -0.21776285266136697
        z: 0.043199425455917845
      - 
        x: -0.0728980308463904
        y: -0.21776748518525976
        z: 0.043178677486599676
      - 
        x: -0.07289751128595456
        y: -0.21777156035442474
        z: 0.04315896491553353
      - 
        x: -0.0728980328188181
        y: -0.2177751558294136
        z: 0.04314018760410865
      - 
        x: -0.0728995156750501
        y: -0.21777834416968683
        z: 0.043122242693275206
      - 
        x: -0.07290189886956136
        y: -0.21778119379305144
        z: 0.043105023928594555
      - 
        x: -0.07290513940403698
        y: -0.2177837699228764
        z: 0.043088420756848
      - 
        x: -0.07290921148455476
        y: -0.21778613551471673
        z: 0.04307231723974478
      - 
        x: -0.07291410576475987
        y: -0.21778835215656003
        z: 0.04305659089084353
      - 
        x: -0.07291982861940322
        y: -0.2177904809539264
        z: 0.04304111172397962
      - 
        x: -0.07292640142019641
        y: -0.21779258347635488
        z: 0.04302574233969121
      - 
        x: -0.07293385961575971
        y: -0.21779472305912229
        z: 0.04301034146169236
      - 
        x: -0.07294225067231458
        y: -0.21779696744024504
        z: 0.04299477801123315
      - 
        x: -0.07295162692062729
        y: -0.21779939583850114
        z: 0.042978976623720244
      - 
        x: -0.07296201766757707
        y: -0.21780212013348318
        z: 0.042963056439793756
      - 
        x: -0.07297332061232817
        y: -0.21780535007345453
        z: 0.04294774660481693
      - 
        x: -0.07298488771233672
        y: -0.21780959545747486
        z: 0.04293562462064966
      - 
        x: -0.07299397381910194
        y: -0.21781629540171826
        z: 0.04293480642901058
      - 
        x: -0.07299057227248706
        y: -0.2178297304136793
        z: 0.04296995474299256
      - 
        x: -0.07292834569165343
        y: -0.21785980975840827
        z: 0.043109427393466376
      - 
        x: -0.07266753624971423
        y: -0.21756370727657526
        z: 0.04315585904339317
    colors: []
    text: ''
    mesh_resource: ''
    mesh_use_embedded_materials: False
...
elpimous commented 1 year ago

My bad !! Read function was ok, Write function had joints position inverted.