Closed elpimous closed 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%)
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
...
My bad !! Read function was ok, Write function had joints position inverted.
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](https://user-images.githubusercontent.com/8529940/230883224-b8c6df7b-a67b-4415-b7f0-f1ef89785bb5.svg)
Regarding to imu, it feeds correctly UnitreeHW.cpp, under sensor_msgs imu_message my xacro file use this imu link name :
<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