echandler5956f
commented
11 months ago The root of the problem was discovered in the fintegrate function (examples/traj_test.cpp). The function calls to Casadi and Pinocchio in the C++ implementation and the Python version (see python/huron_centroidal_v4 in compute_casadi_graphs) are identical—the issue is in what appears to be a differing result from Pinocchio's integrate function for Lie Groups. As a temporary measure, the Python version has been hardcoded into a function called custom_fint in examples/traj_test.cpp, which might (?) only be valid for Huron (although, we only care about the quaternion integration for the floating base, so maybe this hardcoded function will work for all legged robots?).
The Python Bindings for integrate can be found here, and the C++ implementation is here. Note that this issue was verified to occur on both pinocchio3-preview and the main branch
TODO: Find a permanent solution/validate if this approach generalizes
Logs:
The output of tmp1 (the Casadi representation of pinocchio::integrate(cmodel, cq_AD, cq_d_AD * cdt) for C++) and cpin.integrate(s.cmodel, s.cq, s.cq_d * s.cdt) (for Python) is shown below:
C++ version:
@1=1,
@2=2,
@3=(@2*x_16),
@4=(@3*x_16),
@5=(@2*x_17),
@6=(@5*x_17),
@7=(@1-(@4+@6)),
@8=((sq((dx_15*dt))+(sq((dx_16*dt))+sq((dx_17*dt))))+4.93038e-32),
@9=sqrt(@8),
@10=0.00012207,
@11=(@9<@10),
@12=((@11?(@1-(@8/6)):0)+((!@11)?(sin(@9)/@9):0)),
@13=(@9<@10),
@14=(((@13?(0.166667-(@8/120)):0)+((!@13)?((@1-@12)/@8):0))*(((dx_15*dt)*(dx_12*dt))+(((dx_16*dt)*(dx_13*dt))+((dx_17*dt)*(dx_14*dt))))),
@15=(@9<@10),
@16=0.5,
@17=cos(@9),
@18=((@15?(@16-(@8/24)):0)+((!@15)?((@1-@17)/@8):0)),
@19=(((@12*(dx_12*dt))+(@14*(dx_15*dt)))+(@18*(((dx_16*dt)*(dx_14*dt))-((dx_17*dt)*(dx_13*dt))))),
@20=(@3*x_15),
@21=(@5*x_18),
@22=(@20-@21),
@23=(((@12*(dx_13*dt))+(@14*(dx_16*dt)))+(@18*(((dx_17*dt)*(dx_12*dt))-((dx_15*dt)*(dx_14*dt))))),
@24=(@5*x_15),
@25=(@3*x_18),
@26=(@24+@25),
@27=(((@12*(dx_14*dt))+(@14*(dx_17*dt)))+(@18*(((dx_15*dt)*(dx_13*dt))-((dx_16*dt)*(dx_12*dt))))),
@28=(@20+@21),
@29=(@2*x_15),
@30=(@29*x_15),
@31=(@1-(@30+@6)),
@32=(@5*x_16),
@33=(@29*x_18),
@34=(@32-@33),
@35=(@24-@25),
@36=(@32+@33),
@37=(@1-(@30+@4)),
@38=0,
@39=(@18*(dx_15*dt)),
@40=(dx_15*dt),
@41=(@9<@10),
@42=((@41?(@1-(@8/@2)):0)+((!@41)?@17:0)),
@43=((@39*@40)+@42),
@44=(@18*(dx_16*dt)),
@45=((@44*@40)+(@12*(dx_17*dt))),
@46=(@18*(dx_17*dt)),
@47=((@46*@40)-(@12*(dx_16*dt))),
@48=((@7*@43)+((@22*@45)+(@26*@47))),
@49=(dx_16*dt),
@50=((@39*@49)-(@12*(dx_17*dt))),
@51=((@44*@49)+@42),
@52=((@46*@49)+(@12*(dx_15*dt))),
@53=((@28*@50)+((@31*@51)+(@34*@52))),
@54=(dx_17*dt),
@55=((@39*@54)+(@12*(dx_16*dt))),
@56=((@44*@54)-(@12*(dx_15*dt))),
@57=((@46*@54)+@42),
@58=((@35*@55)+((@36*@56)+(@37*@57))),
@59=(@48+(@53+@58)),
@60=(@38<@59),
@61=((@35*@50)+((@36*@51)+(@37*@52))),
@62=((@28*@55)+((@31*@56)+(@34*@57))),
@63=sqrt((@59+@1)),
@64=(@16/@63),
@65=(@48<@53),
@66=((@65&&(@53<@58))||(@48<@58)),
@67=((@7*@55)+((@22*@56)+(@26*@57))),
@68=((@35*@43)+((@36*@45)+(@37*@47))),
@69=sqrt((((@58-@48)-@53)+@1)),
@70=(@16/@69),
@71=((@7*@50)+((@22*@51)+(@26*@52))),
@72=((@28*@43)+((@31*@45)+(@34*@47))),
@73=sqrt((((@53-@58)-@48)+@1)),
@74=(@16/@73),
@75=sqrt((((@48-@53)-@58)+@1)),
@76=((@60?((@61-@62)*@64):0)+((!@60)?((@66?((@67+@68)*@70):0)+((!@66)?((@65?((@71+@72)*@74):0)+((!@65)?(@16*@75):0)):0)):0)),
@77=(@16/@75),
@78=((@60?((@67-@68)*@64):0)+((!@60)?((@66?((@62+@61)*@70):0)+((!@66)?((@65?(@16*@73):0)+((!@65)?((@72+@71)*@77):0)):0)):0)),
@79=((@60?((@72-@71)*@64):0)+((!@60)?((@66?(@16*@69):0)+((!@66)?((@65?((@61+@62)*@74):0)+((!@65)?((@68+@67)*@77):0)):0)):0)),
@80=((@60?(@16*@63):0)+((!@60)?((@66?((@72-@71)*@70):0)+((!@66)?((@65?((@67-@68)*@74):0)+((!@65)?((@61-@62)*@77):0)):0)):0)),
@81=(((@76*x_15)+(@78*x_16))+((@79*x_17)+(@80*x_18))),
@82=(@81<@38),
@83=((@82?(-@76):0)+((!@82)?@76:0)),
@84=(@81<@38),
@85=((@84?(-@78):0)+((!@84)?@78:0)),
@86=(@81<@38),
@87=((@86?(-@79):0)+((!@86)?@79:0)),
@88=(@81<@38),
@89=((@88?(-@80):0)+((!@88)?@80:0)),
@90=((3-((sq(@83)+sq(@85))+(sq(@87)+sq(@89))))/@2),
[
(x_12+((@7*@19)+((@22*@23)+(@26*@27)))),
(x_13+((@28*@19)+((@31*@23)+(@34*@27)))),
(x_14+((@35*@19)+((@36*@23)+(@37*@27)))),
(@83*@90),
(@85*@90),
(@87*@90),
(@89*@90),
(x_19+(dx_18*dt)),
(x_20+(dx_19*dt)),
(x_21+(dx_20*dt)),
(x_22+(dx_21*dt)),
(x_23+(dx_22*dt)),
(x_24+(dx_23*dt)),
(x_25+(dx_24*dt)),
(x_26+(dx_25*dt)),
(x_27+(dx_26*dt)),
(x_28+(dx_27*dt)),
(x_29+(dx_28*dt)),
(x_30+(dx_29*dt))
]Python (Correct) Version:
@1=(dx_12*dt),
@2=(dx_15*dt),
@3=(dx_16*dt),
@4=(dx_17*dt),
@5=4.93038e-32,
@6=((sq(@2)+(sq(@3)+sq(@4)))+@5),
@7=sqrt(@6),
@8=0.00012207,
@9=(@7<@8),
@10=0.5,
@11=24,
@12=1,
@13=((@9?(@10-(@6/@11)):0)+((!@9)?((@12-cos(@7))/@6):0)),
@14=(dx_14*dt), @15=(dx_13*dt),
@16=(@7<@8),
@17=120,
@18=((@16?(0.166667-(@6/@17)):0)+((!@16)?(((@7-sin(@7))/@6)/@7):0)),
@19=((@2*@15)-(@3*@1)),
@20=((@4*@1)-(@2*@14)),
@21=((@1+(@13*((@3*@14)-(@4*@15))))+(@18*((@3*@19)-(@4*@20)))),
@22=((@3*@14)-(@4*@15)),
@23=((@14+(@13*((@2*@15)-(@3*@1))))+(@18*((@2*@20)-(@3*@22)))),
@24=((@15+(@13*((@4*@1)-(@2*@14))))+(@18*((@4*@22)-(@2*@19)))),
@25=((x_16*@23)-(x_17*@24)),
@26=(@25+@25),
@27=((x_15*@24)-(x_16*@21)),
@28=(@27+@27),
@29=((x_17*@21)-(x_15*@23)),
@30=(@29+@29),
@31=(sq(@2)+(sq(@3)+sq(@4))),
@32=(@8<@31),
@33=sqrt((@31+@5)),
@34=(@10*@33),
@35=sin(@34),
@36=(@31/4),
@37=(@10*((@12-(@36/6))+(sq(@36)/@17))),
@38=((@32?(@35*(@2/@33)):0)+((!@32)?(@37*@2):0)),
@39=(@8<@31),
@40=2,
@41=((@39?cos(@34):0)+((!@39)?((@12-(@36/@40))+(sq(@36)/@11)):0)),
@42=(@8<@31),
@43=((@42?(@35*(@4/@33)):0)+((!@42)?(@37*@4):0)),
@44=(@8<@31),
@45=((@44?(@35*(@3/@33)):0)+((!@44)?(@37*@3):0)),
@46=((((x_18*@38)+(x_15*@41))+(x_16*@43))-(x_17*@45)),
@47=((((x_18*@45)+(x_16*@41))+(x_17*@38))-(x_15*@43)),
@48=((((x_18*@43)+(x_17*@41))+(x_15*@45))-(x_16*@38)),
@49=((((x_18*@41)-(x_15*@38))-(x_16*@45))-(x_17*@43)),
@50=(((@46*x_15)+(@47*x_16))+((@48*x_17)+(@49*x_18))),
@51=0,
@52=(@50<@51),
@53=((@52?(-@46):0)+((!@52)?@46:0)),
@54=(@50<@51),
@55=((@54?(-@47):0)+((!@54)?@47:0)),
@56=(@50<@51),
@57=((@56?(-@48):0)+((!@56)?@48:0)),
@58=(@50<@51),
@59=((@58?(-@49):0)+((!@58)?@49:0)),
@60=((3-((sq(@53)+sq(@55))+(sq(@57)+sq(@59))))/@40),
[
(((@21+(x_18*@26))+((x_16*@28)-(x_17*@30)))+x_12),
(((@24+(x_18*@30))+((x_17*@26)-(x_15*@28)))+x_13),
(((@23+(x_18*@28))+((x_15*@30)-(x_16*@26)))+x_14),
(@53*@60),
(@55*@60),
(@57*@60),
(@59*@60),
(x_19+(dx_18*dt)),
(x_20+(dx_19*dt)),
(x_21+(dx_20*dt)),
(x_22+(dx_21*dt)),
(x_23+(dx_22*dt)),
(x_24+(dx_23*dt)),
(x_25+(dx_24*dt)),
(x_26+(dx_25*dt)),
(x_27+(dx_26*dt)),
(x_28+(dx_27*dt)),
(x_29+(dx_28*dt)),
(x_30+(dx_29*dt))
]
Is my setup correct? Dynamics are identical to
python/huron_centroidal_v4.py. PseudospectralSegment and TrajOpt seem to work without bugs for rosenbrock.TODO: Fix asap