├── .gitmodules ├── README.md ├── autogenerated ├── singleBodyParamHessian.m └── singleBodyParamJacobian.m ├── data ├── Cheetah3LegModel.m └── CheetahSysID.mat ├── init_path.m ├── main.m ├── utiltiy ├── QtoSemiAxes.m ├── SemiAxesToQ.m ├── composedJacobianAndHessian.m ├── logCholeskyVecToParams.m ├── regularizedLeastSquares.m ├── rms.m ├── splitParams.m ├── systemParamHessian.m ├── systemParamJacobian.m └── systemParamMap.m └── vis ├── Cheetah3_prior_inertia_CAD.m ├── Cheetah_bound_visualize.m ├── Cheetah_ellipsoid_visualize.m ├── c3_body.obj ├── c3_lower_link.obj ├── c3_upper_link.obj ├── draw_SE3.m ├── plot_bounding_ellipsoid.m ├── plot_inertiatensor.m └── readObj.m /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "spatial_v2_extended"] 2 | path = spatial_v2_extended 3 | url = git@github.com:ROAM-Lab-ND/spatial_v2_extended.git 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Inertia Identification Minimal Examples 2 | 3 | This repository contains a minimal working MATLAB example of the methods described in: 4 | 5 | *Smooth Parameterization of Rigid-Body Inertia* by Rucker and Wensing ([link](https://ieeexplore.ieee.org/document/9690029e)) 6 | 7 | and 8 | 9 | *Geometric Robot Dynamic Identification: A Convex Programming Approach* by Lee, Wensing, and Park ([link](https://ieeexplore.ieee.org/document/8922724), [supplemental repo](https://github.com/alex07143/Geometric-Robot-DynID)) 10 | 11 | The repository requires you to have [CVX](http://cvxr.com/cvx/) installed and accessible in your MATLAB path. 12 | 13 | After pulling down the code, be sure to pull in the submodule using `git submodule update --init --recursive` 14 | 15 | Please email me at [pwensing@nd.edu](mailto:pwensing@nd.edu) if you have any issues, or feel free to open an issue. 16 | -------------------------------------------------------------------------------- /autogenerated/singleBodyParamHessian.m: -------------------------------------------------------------------------------- 1 | function Hparam = singleBodyParamHessian(in1,in2) 2 | %SINGLEBODYPARAMHESSIAN 3 | % HPARAM = SINGLEBODYPARAMHESSIAN(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.4. 6 | % 06-Sep-2021 00:30:19 7 | 8 | a = in1(10,:); 9 | d1 = in1(1,:); 10 | d2 = in1(2,:); 11 | d3 = in1(3,:); 12 | pi01 = in2(1,:); 13 | pi02 = in2(2,:); 14 | pi03 = in2(3,:); 15 | pi04 = in2(4,:); 16 | pi05 = in2(5,:); 17 | pi06 = in2(6,:); 18 | pi07 = in2(7,:); 19 | pi08 = in2(8,:); 20 | pi09 = in2(9,:); 21 | pi010 = in2(10,:); 22 | s12 = in1(4,:); 23 | s13 = in1(5,:); 24 | s23 = in1(6,:); 25 | t1 = in1(7,:); 26 | t2 = in1(8,:); 27 | t3 = in1(9,:); 28 | t5 = exp(a); 29 | t6 = exp(d1); 30 | t7 = exp(d2); 31 | t8 = exp(d3); 32 | t9 = a.*2.0; 33 | t10 = d1.*2.0; 34 | t11 = d2.*2.0; 35 | t12 = d3.*2.0; 36 | t13 = s12.^2; 37 | t14 = s13.^2; 38 | t15 = s23.^2; 39 | t16 = t1.^2; 40 | t34 = pi05./2.0; 41 | t35 = pi06./2.0; 42 | t36 = pi07./2.0; 43 | t17 = t5.^2; 44 | t18 = t7.^2; 45 | t19 = t8.^2; 46 | t20 = pi04.*s23.*t5; 47 | t21 = pi08.*s23.*t5; 48 | t22 = pi09.*s23.*t5; 49 | t23 = pi01.*t2.*t5; 50 | t24 = pi01.*t3.*t5; 51 | t25 = pi02.*t2.*t5; 52 | t26 = pi02.*t3.*t5; 53 | t27 = pi03.*t2.*t5; 54 | t28 = pi03.*t3.*t5; 55 | t29 = pi04.*t2.*t5; 56 | t30 = pi04.*t3.*t5; 57 | t31 = d1+t9; 58 | t32 = d2+t9; 59 | t33 = d3+t9; 60 | t37 = pi03.*t5.*t7; 61 | t38 = pi04.*t5.*t8; 62 | t39 = pi08.*t5.*t7; 63 | t40 = pi08.*t5.*t8; 64 | t41 = pi09.*t5.*t8; 65 | t42 = pi010.*t5.*t7; 66 | t43 = -t35; 67 | t44 = -t36; 68 | t57 = t9+t10; 69 | t58 = t9+t11; 70 | t59 = t9+t12; 71 | t45 = exp(t31); 72 | t46 = exp(t32); 73 | t47 = exp(t33); 74 | t48 = pi01.*t17; 75 | t49 = pi03.*t17; 76 | t50 = pi04.*t17; 77 | t51 = pi05.*t17; 78 | t52 = pi06.*t17; 79 | t53 = pi07.*t17; 80 | t54 = pi08.*t17; 81 | t55 = -t21; 82 | t56 = -t25; 83 | t64 = exp(t57); 84 | t65 = exp(t58); 85 | t66 = exp(t59); 86 | t67 = -t39; 87 | t68 = -t40; 88 | t69 = -t41; 89 | t99 = pi02.*t6.*t17; 90 | t103 = pi09.*t6.*t17; 91 | t158 = pi010.*t6.*t7.*t17; 92 | t213 = t24+t38; 93 | t217 = t34+t36+t43; 94 | t218 = t34+t35+t44; 95 | t219 = t20+t23+t37; 96 | t60 = t48.*2.0; 97 | t61 = t49.*2.0; 98 | t62 = t50.*2.0; 99 | t63 = t54.*2.0; 100 | t71 = s12.*t49.*4.0; 101 | t73 = s12.*t51.*2.0; 102 | t74 = s13.*t51.*2.0; 103 | t75 = s12.*t52.*2.0; 104 | t76 = s13.*t50.*4.0; 105 | t77 = s13.*t52.*2.0; 106 | t78 = s12.*t53.*2.0; 107 | t79 = s13.*t53.*2.0; 108 | t81 = s12.*t54.*4.0; 109 | t82 = s13.*t54.*4.0; 110 | t84 = s23.*t51.*2.0; 111 | t85 = s23.*t50.*4.0; 112 | t86 = s23.*t52.*2.0; 113 | t87 = s23.*t53.*2.0; 114 | t90 = t1.*t48.*4.0; 115 | t92 = t2.*t48.*4.0; 116 | t94 = t3.*t48.*4.0; 117 | t95 = t1.*t49.*4.0; 118 | t97 = t1.*t50.*4.0; 119 | t98 = t2.*t50.*4.0; 120 | t100 = t7.*t49; 121 | t101 = t8.*t50; 122 | t102 = t7.*t54; 123 | t104 = t8.*t54; 124 | t105 = -t48; 125 | t106 = -t49; 126 | t107 = -t50; 127 | t108 = -t52; 128 | t109 = -t53; 129 | t111 = s12.*t49.*-2.0; 130 | t112 = s13.*t50.*-2.0; 131 | t117 = s23.*t50.*-2.0; 132 | t119 = t1.*t48.*-2.0; 133 | t120 = t2.*t48.*-2.0; 134 | t121 = t1.*t50.*-2.0; 135 | t122 = pi02.*t45.*2.0; 136 | t123 = pi02.*t45.*4.0; 137 | t124 = pi03.*t46.*2.0; 138 | t125 = pi03.*t46.*4.0; 139 | t126 = pi04.*t47.*2.0; 140 | t127 = pi04.*t47.*4.0; 141 | t128 = pi08.*t46.*2.0; 142 | t129 = pi09.*t45.*2.0; 143 | t130 = pi010.*t45.*2.0; 144 | t131 = pi08.*t46.*4.0; 145 | t132 = pi09.*t45.*4.0; 146 | t133 = pi010.*t45.*4.0; 147 | t134 = t99.*2.0; 148 | t138 = t103.*2.0; 149 | t157 = t8.*t103; 150 | t165 = -t99; 151 | t171 = t13.*t51.*2.0; 152 | t172 = t14.*t51.*2.0; 153 | t173 = t13.*t52.*2.0; 154 | t174 = t14.*t52.*2.0; 155 | t175 = t13.*t53.*2.0; 156 | t176 = t14.*t53.*2.0; 157 | t177 = t16.*t48.*4.0; 158 | t178 = s12.*s13.*t54.*8.0; 159 | t179 = s12.*t1.*t49.*8.0; 160 | t180 = s13.*t1.*t50.*8.0; 161 | t181 = pi05.*t64.*2.0; 162 | t182 = pi05.*t65.*2.0; 163 | t183 = pi06.*t64.*2.0; 164 | t184 = pi05.*t66.*2.0; 165 | t185 = pi06.*t65.*2.0; 166 | t186 = pi07.*t64.*2.0; 167 | t187 = pi06.*t66.*2.0; 168 | t188 = pi07.*t65.*2.0; 169 | t189 = pi07.*t66.*2.0; 170 | t190 = pi09.*s13.*t45.*-2.0; 171 | t191 = pi010.*s12.*t45.*-2.0; 172 | t192 = pi09.*s13.*t45.*-4.0; 173 | t193 = pi010.*s12.*t45.*-4.0; 174 | t194 = pi09.*s13.*t45.*8.0; 175 | t195 = pi010.*s12.*t45.*8.0; 176 | t196 = pi08.*s23.*t46.*-4.0; 177 | t197 = pi02.*t1.*t45.*8.0; 178 | t204 = t158.*2.0; 179 | t214 = t26+t69; 180 | t215 = t28+t68; 181 | t216 = t5.*t213.*2.0; 182 | t223 = t22+t42+t56; 183 | t229 = s23.*t5.*t218; 184 | t232 = t5.*t219.*2.0; 185 | t233 = t17.*t218; 186 | t234 = t5.*t7.*t217; 187 | t235 = t5.*t8.*t218; 188 | t237 = t7.*t17.*t217; 189 | t250 = t17.*t18.*t217.*2.0; 190 | t70 = s12.*t61; 191 | t72 = s13.*t62; 192 | t80 = s12.*t63; 193 | t83 = s23.*t62; 194 | t88 = t1.*t60; 195 | t89 = t2.*t60; 196 | t91 = t3.*t60; 197 | t93 = t1.*t62; 198 | t96 = t2.*t62; 199 | t110 = -t63; 200 | t113 = -t75; 201 | t114 = -t79; 202 | t115 = -t81; 203 | t116 = -t82; 204 | t118 = -t87; 205 | t135 = t7.*t61; 206 | t136 = t8.*t62; 207 | t137 = t7.*t63; 208 | t139 = s13.*t129; 209 | t140 = s12.*t130; 210 | t141 = s13.*t132; 211 | t142 = s12.*t133; 212 | t143 = s23.*t131; 213 | t144 = t1.*t122; 214 | t145 = t1.*t123; 215 | t146 = t2.*t125; 216 | t147 = t3.*t127; 217 | t156 = t8.*t102; 218 | t159 = -t128; 219 | t160 = -t129; 220 | t161 = -t130; 221 | t162 = -t131; 222 | t163 = -t132; 223 | t164 = -t133; 224 | t166 = -t134; 225 | t167 = -t100; 226 | t168 = t100.*-2.0; 227 | t169 = -t101; 228 | t170 = t102.*-2.0; 229 | t200 = t1.*t101.*-2.0; 230 | t201 = t2.*t101.*-2.0; 231 | t203 = t8.*t138; 232 | t205 = -t173; 233 | t206 = -t176; 234 | t207 = -t178; 235 | t208 = -t181; 236 | t209 = -t185; 237 | t210 = -t189; 238 | t211 = -t194; 239 | t212 = -t195; 240 | t220 = -t216; 241 | t221 = t94+t127; 242 | t222 = t5.*t215.*2.0; 243 | t225 = t5.*t6.*t214.*2.0; 244 | t227 = t51+t52+t109; 245 | t228 = t51+t53+t108; 246 | t231 = t5.*t7.*t215.*-2.0; 247 | t236 = -t232; 248 | t238 = t8.*t233; 249 | t239 = -t233; 250 | t240 = s13.*t233.*2.0; 251 | t241 = s23.*t233.*2.0; 252 | t242 = t5.*t6.*t223.*2.0; 253 | t244 = -t237; 254 | t246 = s12.*t237.*2.0; 255 | t249 = t85+t92+t125; 256 | t251 = t19.*t233.*2.0; 257 | t255 = t30+t235; 258 | t260 = t71+t76+t90+t123; 259 | t262 = t27+t55+t234; 260 | t263 = t29+t67+t229; 261 | t148 = t8.*t80; 262 | t149 = s13.*t137; 263 | t150 = s23.*t137; 264 | t151 = t1.*t135; 265 | t152 = t2.*t135; 266 | t153 = t8.*t93; 267 | t154 = t8.*t96; 268 | t155 = t3.*t136; 269 | t198 = s23.*t170; 270 | t199 = t1.*t168; 271 | t202 = t8.*t137; 272 | t224 = -t222; 273 | t226 = t7.*t222; 274 | t230 = -t225; 275 | t243 = -t240; 276 | t245 = -t238; 277 | t247 = s13.*t238.*2.0; 278 | t248 = s23.*t238.*2.0; 279 | t252 = -t246; 280 | t256 = t117+t120+t168; 281 | t257 = t5.*t255.*2.0; 282 | t261 = t91+t136+t216; 283 | t264 = t111+t112+t119+t166; 284 | t265 = t5.*t262.*2.0; 285 | t266 = t5.*t263.*2.0; 286 | t270 = t84+t86+t98+t118+t162; 287 | t271 = t147+t184+t187+t210; 288 | t272 = t83+t89+t135+t232; 289 | t275 = t73+t78+t95+t113+t116+t164; 290 | t276 = t74+t77+t97+t114+t115+t163; 291 | t277 = t146+t182+t188+t196+t209; 292 | t280 = t144+t183+t186+t190+t191+t208; 293 | t281 = t145+t183+t186+t192+t193+t208; 294 | t253 = -t247; 295 | t254 = -t248; 296 | t258 = -t257; 297 | t259 = t8.*t257; 298 | t267 = -t265; 299 | t268 = -t266; 300 | t269 = t7.*t265; 301 | t274 = t80+t121+t138+t243; 302 | t278 = t149+t199+t204+t252; 303 | t283 = t96+t170+t241+t266; 304 | t273 = t201+t202+t254; 305 | t279 = t148+t200+t203+t253; 306 | t282 = t155+t251+t259; 307 | t284 = t152+t198+t250+t269; 308 | Hparam = reshape([0.0,t99,0.0,0.0,0.0,t280,t280,0.0,-t5.*t6.*t214,t5.*t6.*t223,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t158,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t157,0.0,0.0,0.0,0.0,0.0,0.0,t161,t161,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t160,t160,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t103,0.0,0.0,0.0,0.0,0.0,t122,t122,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t165,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t165,0.0,0.0,t134,0.0,0.0,0.0,t281,t281,0.0,t230,t242,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t158,0.0,0.0,t100,0.0,-s23.*t102+t2.*t100+t17.*t18.*t217.*3.0+t5.*t7.*t262,0.0,t182+t188+t209+t2.*t124-pi08.*s23.*t46.*2.0,-t5.*t7.*t215,0.0,t158+s13.*t102+s12.*t244+t1.*t167,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t156,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t244,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t102,0.0,0.0,0.0,0.0,t170,0.0,t159,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t167,0.0,0.0,0.0,0.0,t135,0.0,t124,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t167,0.0,0.0,0.0,0.0,t135,0.0,t284,0.0,t277,t231,0.0,t278,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t157,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t156,0.0,0.0,0.0,0.0,0.0,t101,t3.*t101+t19.*t233.*3.0+t5.*t8.*t255,t184+t187+t210+t3.*t126,0.0,t156+s23.*t245+t2.*t169,t157+s12.*t104+s13.*t245+t1.*t169,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t104,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t245,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t245,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t169,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t169,0.0,0.0,0.0,0.0,0.0,0.0,t136,t126,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t136,t282,t271,0.0,t273,t279,0.0,0.0,0.0,0.0,0.0,0.0,t161,t161,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t244,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t104,0.0,0.0,0.0,0.0,0.0,0.0,t228,t228,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t110,t110,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t54,0.0,0.0,0.0,0.0,0.0,t61,t61,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t106,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t106,0.0,0.0,t61,0.0,0.0,0.0,t275,t275,0.0,t224,t267,0.0,0.0,0.0,0.0,0.0,t160,t160,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t102,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t245,0.0,0.0,0.0,0.0,0.0,0.0,t110,t110,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t227,t227,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t239,0.0,0.0,0.0,0.0,0.0,t62,t62,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t107,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t107,0.0,0.0,t62,0.0,0.0,0.0,t276,t276,0.0,t258,t268,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t103,0.0,0.0,0.0,0.0,t170,0.0,t159,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t245,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t54,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t239,0.0,0.0,0.0,0.0,t233.*2.0,0.0,t227,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t107,0.0,0.0,0.0,0.0,t62,0.0,t62,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t107,0.0,0.0,0.0,0.0,t62,0.0,t283,0.0,t270,t258,0.0,t274,0.0,0.0,0.0,0.0,0.0,t122,t122,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t167,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t169,0.0,0.0,0.0,0.0,0.0,0.0,t61,t61,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t62,t62,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t107,0.0,0.0,0.0,0.0,0.0,t60,t60,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t105,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t105,0.0,0.0,t60,0.0,0.0,0.0,t260,t260,0.0,t220,t236,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t165,0.0,0.0,0.0,0.0,t135,0.0,t124,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t169,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t106,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t107,0.0,0.0,0.0,0.0,t62,0.0,t62,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t105,0.0,0.0,0.0,0.0,t60,0.0,t60,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t105,0.0,0.0,0.0,0.0,t60,0.0,t272,0.0,t249,t220,0.0,t264,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t165,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t167,0.0,0.0,0.0,0.0,0.0,0.0,t136,t126,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t106,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t107,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t107,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t105,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t105,0.0,0.0,0.0,0.0,0.0,0.0,t60,t60,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t60,t261,t221,0.0,t256,t264,0.0,0.0,t134,0.0,0.0,0.0,t281,t281,0.0,t230,t242,0.0,0.0,t135,0.0,t284,0.0,t277,t231,0.0,t278,0.0,0.0,0.0,t136,t282,t271,0.0,t273,t279,0.0,0.0,t61,0.0,0.0,0.0,t275,t275,0.0,t224,t267,0.0,t62,0.0,0.0,0.0,t276,t276,0.0,t258,t268,0.0,0.0,t62,0.0,t283,0.0,t270,t258,0.0,t274,0.0,t60,0.0,0.0,0.0,t260,t260,0.0,t220,t236,0.0,0.0,t60,0.0,t272,0.0,t249,t220,0.0,t264,0.0,0.0,0.0,t60,t261,t221,0.0,t256,t264,0.0,t48.*4.0,t5.*(pi03.*s12.*t5+pi04.*s13.*t5+pi01.*t1.*t5+pi02.*t5.*t6).*4.0,t5.*t219.*4.0,t5.*t213.*4.0,s23.*t5.*t263.*4.0+t3.*t5.*t213.*4.0+t2.*t5.*t219.*4.0+t5.*t8.*t255.*4.0+t5.*t7.*t262.*4.0,t171+t172+t174+t175+t177+t179+t180+t183+t184+t186+t187+t197+t205+t206+t207+t208+t210+t211+t212+t3.*t94+pi04.*t3.*t47.*8.0,t171+t172+t174+t175+t177+t179+t180+t182+t183+t186+t188+t197+t205+t206+t207+t208+t209+t211+t212+t15.*t51.*2.0+t15.*t52.*2.0-t15.*t53.*2.0+t2.*t92-pi08.*s23.*t46.*8.0+pi03.*t2.*t46.*8.0+s23.*t2.*t50.*8.0,s23.*t5.*t255.*-4.0-t2.*t5.*t213.*4.0-t5.*t7.*t215.*4.0,s12.*t5.*t215.*-4.0-s13.*t5.*t255.*4.0-t1.*t5.*t213.*4.0-t5.*t6.*t214.*4.0,s12.*t5.*t262.*-4.0-s13.*t5.*t263.*4.0-t1.*t5.*t219.*4.0+t5.*t6.*t223.*4.0],[10,10,10]); 309 | -------------------------------------------------------------------------------- /autogenerated/singleBodyParamJacobian.m: -------------------------------------------------------------------------------- 1 | function Jparam = singleBodyParamJacobian(in1,in2) 2 | %SINGLEBODYPARAMJACOBIAN 3 | % JPARAM = SINGLEBODYPARAMJACOBIAN(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.4. 6 | % 06-Sep-2021 00:30:12 7 | 8 | a = in1(10,:); 9 | d1 = in1(1,:); 10 | d2 = in1(2,:); 11 | d3 = in1(3,:); 12 | pi01 = in2(1,:); 13 | pi02 = in2(2,:); 14 | pi03 = in2(3,:); 15 | pi04 = in2(4,:); 16 | pi05 = in2(5,:); 17 | pi06 = in2(6,:); 18 | pi07 = in2(7,:); 19 | pi08 = in2(8,:); 20 | pi09 = in2(9,:); 21 | pi010 = in2(10,:); 22 | s12 = in1(4,:); 23 | s13 = in1(5,:); 24 | s23 = in1(6,:); 25 | t1 = in1(7,:); 26 | t2 = in1(8,:); 27 | t3 = in1(9,:); 28 | t5 = exp(a); 29 | t6 = exp(d1); 30 | t7 = exp(d2); 31 | t8 = exp(d3); 32 | t9 = a.*2.0; 33 | t10 = d1.*2.0; 34 | t11 = d2.*2.0; 35 | t12 = d3.*2.0; 36 | t13 = s12.^2; 37 | t14 = s13.^2; 38 | t15 = s23.^2; 39 | t16 = t1.^2; 40 | t32 = pi05./2.0; 41 | t33 = pi06./2.0; 42 | t34 = pi07./2.0; 43 | t17 = t5.^2; 44 | t18 = pi04.*s23.*t5; 45 | t19 = pi08.*s23.*t5; 46 | t20 = pi09.*s23.*t5; 47 | t21 = pi01.*t2.*t5; 48 | t22 = pi01.*t3.*t5; 49 | t23 = pi02.*t2.*t5; 50 | t24 = pi02.*t3.*t5; 51 | t25 = pi03.*t2.*t5; 52 | t26 = pi03.*t3.*t5; 53 | t27 = pi04.*t2.*t5; 54 | t28 = pi04.*t3.*t5; 55 | t29 = d1+t9; 56 | t30 = d2+t9; 57 | t31 = d3+t9; 58 | t35 = pi03.*t5.*t7; 59 | t36 = pi04.*t5.*t8; 60 | t37 = pi08.*t5.*t7; 61 | t38 = pi08.*t5.*t8; 62 | t39 = pi09.*t5.*t8; 63 | t40 = pi010.*t5.*t7; 64 | t41 = -t33; 65 | t42 = -t34; 66 | t61 = t9+t10; 67 | t62 = t9+t11; 68 | t63 = t9+t12; 69 | t43 = exp(t29); 70 | t44 = exp(t30); 71 | t45 = exp(t31); 72 | t46 = pi01.*t17; 73 | t47 = pi04.*t17; 74 | t48 = -t19; 75 | t49 = -t23; 76 | t50 = pi03.*s12.*t17; 77 | t52 = pi05.*s12.*t17; 78 | t53 = pi05.*s13.*t17; 79 | t54 = pi06.*s12.*t17; 80 | t55 = pi06.*s13.*t17; 81 | t56 = pi07.*s12.*t17; 82 | t57 = pi07.*s13.*t17; 83 | t64 = exp(t61); 84 | t65 = exp(t62); 85 | t66 = exp(t63); 86 | t67 = -t37; 87 | t68 = -t38; 88 | t69 = -t39; 89 | t72 = pi08.*s12.*t17.*2.0; 90 | t73 = pi08.*s13.*t17.*2.0; 91 | t75 = pi03.*t1.*t17.*2.0; 92 | t77 = pi02.*t6.*t17; 93 | t78 = pi03.*t7.*t17; 94 | t90 = pi05.*t13.*t17; 95 | t91 = pi05.*t14.*t17; 96 | t92 = pi06.*t13.*t17; 97 | t93 = pi06.*t14.*t17; 98 | t94 = pi07.*t13.*t17; 99 | t95 = pi07.*t14.*t17; 100 | t96 = pi08.*s12.*s13.*t17.*4.0; 101 | t128 = t22+t36; 102 | t132 = t32+t34+t41; 103 | t133 = t32+t33+t42; 104 | t134 = t18+t21+t35; 105 | t51 = s13.*t47; 106 | t58 = s23.*t47; 107 | t59 = t1.*t46; 108 | t60 = t2.*t46; 109 | t70 = t50.*2.0; 110 | t76 = t1.*t47.*2.0; 111 | t79 = t8.*t47; 112 | t80 = -t50; 113 | t82 = -t54; 114 | t83 = -t57; 115 | t84 = -t72; 116 | t85 = -t73; 117 | t87 = pi02.*t43.*2.0; 118 | t88 = pi09.*t43.*2.0; 119 | t89 = pi010.*t43.*2.0; 120 | t97 = t1.*t50.*4.0; 121 | t99 = pi05.*t64; 122 | t100 = pi05.*t65; 123 | t101 = pi06.*t64; 124 | t102 = pi05.*t66; 125 | t103 = pi06.*t65; 126 | t104 = pi07.*t64; 127 | t105 = pi06.*t66; 128 | t106 = pi07.*t65; 129 | t107 = pi07.*t66; 130 | t110 = pi09.*s13.*t43.*4.0; 131 | t111 = pi010.*s12.*t43.*4.0; 132 | t113 = pi02.*t1.*t43.*4.0; 133 | t116 = -t77; 134 | t117 = t16.*t46.*2.0; 135 | t118 = -t96; 136 | t119 = pi09.*s13.*t43.*-2.0; 137 | t120 = pi010.*s12.*t43.*-2.0; 138 | t123 = -t92; 139 | t124 = -t95; 140 | t129 = t24+t69; 141 | t130 = t26+t68; 142 | t131 = t5.*t128; 143 | t136 = t20+t40+t49; 144 | t137 = t5.*t134; 145 | t138 = s23.*t5.*t133; 146 | t139 = t5.*t7.*t132; 147 | t140 = t5.*t8.*t133; 148 | t71 = t51.*2.0; 149 | t74 = t59.*2.0; 150 | t81 = -t51; 151 | t86 = -t59; 152 | t98 = t1.*t51.*4.0; 153 | t108 = s13.*t88; 154 | t109 = s12.*t89; 155 | t112 = t1.*t87; 156 | t114 = -t88; 157 | t115 = -t89; 158 | t121 = -t110; 159 | t122 = -t111; 160 | t125 = -t99; 161 | t126 = -t103; 162 | t127 = -t107; 163 | t135 = -t131; 164 | t141 = t28+t140; 165 | t145 = t25+t48+t139; 166 | t146 = t27+t67+t138; 167 | t142 = t5.*t141; 168 | t144 = t70+t71+t74+t87; 169 | t147 = t5.*t146; 170 | t148 = t80+t81+t86+t116; 171 | t149 = t52+t56+t75+t82+t85+t115; 172 | t150 = t53+t55+t76+t83+t84+t114; 173 | t151 = t101+t104+t112+t119+t120+t125; 174 | t143 = -t142; 175 | Jparam = reshape([0.0,t77,0.0,0.0,0.0,t151,t151,0.0,-t5.*t6.*t129,t5.*t6.*t136,0.0,0.0,t78,0.0,t2.*t78+t7.^2.*t17.*t132+t5.*t7.*t145-pi08.*s23.*t7.*t17,0.0,t100+t106+t126-pi08.*s23.*t44.*2.0+pi03.*t2.*t44.*2.0,-t5.*t7.*t130,0.0,-t1.*t78+pi08.*s13.*t7.*t17+pi010.*t6.*t7.*t17-s12.*t7.*t17.*t132,0.0,0.0,0.0,t79,t3.*t79+t8.*t142+t8.^2.*t17.*t133,t102+t105+t127+pi04.*t3.*t45.*2.0,0.0,-t2.*t79+pi08.*t7.*t8.*t17-s23.*t8.*t17.*t133,-t1.*t79+pi08.*s12.*t8.*t17+pi09.*t6.*t8.*t17-s13.*t8.*t17.*t133,0.0,0.0,pi03.*t17,0.0,0.0,0.0,t149,t149,0.0,-t5.*t130,-t5.*t145,0.0,t47,0.0,0.0,0.0,t150,t150,0.0,t143,-t147,0.0,0.0,t47,0.0,t147+t2.*t47-pi08.*t7.*t17+s23.*t17.*t133,0.0,pi08.*t44.*-2.0+t2.*t47.*2.0+pi05.*s23.*t17+pi06.*s23.*t17-pi07.*s23.*t17,t143,0.0,-t1.*t47+pi08.*s12.*t17+pi09.*t6.*t17-s13.*t17.*t133,0.0,t46,0.0,0.0,0.0,t144,t144,0.0,t135,-t137,0.0,0.0,t46,0.0,t58+t60+t78+t137,0.0,t58.*2.0+t60.*2.0+pi03.*t44.*2.0,t135,0.0,t148,0.0,0.0,0.0,t46,t79+t131+t3.*t46,pi04.*t45.*2.0+t3.*t46.*2.0,0.0,-t58-t60-t78,t148,0.0,t46.*2.0,t5.*(pi03.*s12.*t5+pi04.*s13.*t5+pi01.*t1.*t5+pi02.*t5.*t6).*2.0,t137.*2.0,t131.*2.0,s23.*t147.*2.0+t3.*t131.*2.0+t2.*t137.*2.0+t8.*t142.*2.0+t5.*t7.*t145.*2.0,t90+t91+t93+t94+t97+t98+t101+t102+t104+t105+t113+t117+t118+t121+t122+t123+t124+t125+t127+t3.^2.*t46.*2.0+pi04.*t3.*t45.*4.0,t90+t91+t93+t94+t97+t98+t100+t101+t104+t106+t113+t117+t118+t121+t122+t123+t124+t125+t126+t2.*t58.*4.0+t2.*t60.*2.0-pi08.*s23.*t44.*4.0+pi05.*t15.*t17+pi06.*t15.*t17-pi07.*t15.*t17+pi03.*t2.*t44.*4.0,s23.*t142.*-2.0-t2.*t131.*2.0-t5.*t7.*t130.*2.0,s13.*t142.*-2.0-t1.*t131.*2.0-s12.*t5.*t130.*2.0-t5.*t6.*t129.*2.0,s13.*t147.*-2.0-t1.*t137.*2.0-s12.*t5.*t145.*2.0+t5.*t6.*t136.*2.0],[10,10]); 176 | -------------------------------------------------------------------------------- /data/Cheetah3LegModel.m: -------------------------------------------------------------------------------- 1 | function [model, graphics] = Cheetah3LegModel() 2 | 3 | % This function creates both the rigid body model struct and a graphics 4 | % cell array that specifies the cheetah 3 model 5 | 6 | % The 0-configuration for the robot is with legs stright down, cheetah 7 | % pointed along the +x axis of the ICS. 8 | % The inertial coordinates have +z up (i.e. gravity = [0 0 -9.81 m/s^2]) 9 | % The body coordinates have +x forward, +y left, +z up 10 | % 11 | 12 | model.gc.point = zeros(3,0); 13 | model.gc.body = zeros(1,0); 14 | 15 | Nb = 6; 16 | 17 | %% Nominal Paramters of Cheetah 18 | bodyWidth = .256; 19 | bodyHeight = .200; 20 | bodyLength = .600; 21 | 22 | 23 | motorRad = .06; 24 | legRad = 0.03; 25 | 26 | lo = 0.045; 27 | lc = bodyWidth/2; 28 | l1 = .342; % Length of top link 29 | l2 = .345; % Length of bottom link 30 | motorWidth = 2*lo; 31 | 32 | 33 | %% Legs 34 | side_sign = 1; 35 | 36 | Nb = 0; 37 | side_sign = 1; 38 | %% Legs 39 | 40 | %% Ab/Ad (Hip Roll) 41 | Nb = Nb+1; 42 | 43 | model.parent(Nb) = 0; 44 | model.jtype{Nb} = 'Rx'; 45 | model.jtype_rotor{Nb} = 'Rx'; 46 | model.Xtree{Nb} = plux(eye(3), [0 0 0]); 47 | model.Xrotor{Nb} = plux(eye(3), [0 -side_sign*lc 0]'); 48 | 49 | model.I{Nb} = mcI(1.5, [0 side_sign*lo 0], boxInertia(1.5, ones(3,1)*motorWidth)); 50 | model.I_rotor{Nb} = mcI(.5, [0 0 0], boxInertia(.5, [.25 1 1]'*motorWidth)); 51 | 52 | graphics{Nb}.boundCenter = [0 side_sign*lo 0]'; 53 | graphics{Nb}.boundAxes = [motorWidth motorWidth motorWidth]/2*1.8; 54 | 55 | graphics{Nb}.boundCenterRot = [0 0 0]'; 56 | graphics{Nb}.boundAxesRot = [motorWidth motorWidth motorWidth]/2*1.8; 57 | 58 | model.gr{Nb} = 10.604; 59 | 60 | 61 | %% Hip Pitch 62 | Nb = Nb+1; 63 | model.parent(Nb) = Nb-1; 64 | model.jtype{Nb} = 'Ry'; 65 | model.jtype_rotor{Nb} = 'Ry'; 66 | model.Xtree{Nb} = plux(rz(pi), [0 side_sign*lo 0]'); 67 | model.Xrotor{Nb} = plux(rz(pi), [0 side_sign*lo 0]'); 68 | 69 | model.I{Nb} = mcI(.5, [0 0 -l1]/2, boxInertia(.5,[legRad*2 legRad*2 l1]) ); 70 | model.I_rotor{Nb} = mcI(.5, [0 0 0]/2, boxInertia(.5, [1 .25 1]'*motorWidth) ); 71 | 72 | model.gr{Nb} = 10.604; 73 | 74 | graphics{Nb}.boundCenter = [0 0 -l1]'/2; 75 | graphics{Nb}.boundAxes = [legRad*2.2 legRad*2.2 l1*1.2]/2*1.8; 76 | 77 | graphics{Nb}.boundCenterRot = [0 0 0]'/2; 78 | graphics{Nb}.boundAxesRot = [motorWidth motorWidth motorWidth]/2*1.8; 79 | 80 | 81 | 82 | %% Knee Pitch 83 | Nb = Nb+1; 84 | model.parent(Nb) = Nb-1; 85 | model.jtype{Nb} = 'Ry'; 86 | model.jtype_rotor{Nb} = 'Ry'; 87 | 88 | 89 | model.Xtree{Nb} = plux(eye(3), [0 0 -l1]'); 90 | model.Xrotor{Nb} = plux(eye(3), [0 0 0 ]'); 91 | 92 | model.I{Nb} = mcI(.5, [0 0 -l2/2], boxInertia(.5,[legRad*2 legRad*2 l2]) ); 93 | model.I_rotor{Nb} = mcI(.5, [0 0 0 ], boxInertia(.5,[1 .25 1]'*motorWidth) ); 94 | model.gr{Nb} = 10.604; 95 | 96 | model.NB = Nb; 97 | 98 | graphics{Nb}.boundCenter = [0 0 -l2]'/2; 99 | graphics{Nb}.boundAxes = [legRad*2 legRad*2 l2*1.2]/2*1.4; 100 | 101 | 102 | graphics{Nb}.boundCenterRot = [0 0 0]'; 103 | graphics{Nb}.boundAxesRot = [motorWidth motorWidth motorWidth]/2*1.8; 104 | 105 | mT = 0; 106 | for i = 1:length(model.I) 107 | mT = mT+model.I{i}(6,6); 108 | end 109 | %mT 110 | 111 | model.NB = Nb; 112 | 113 | model.has_rotor=[1 1 1]'; 114 | model = postProcessModel(model); 115 | 116 | end 117 | 118 | function I = boxInertia(mass, x) 119 | I = (norm(x)^2*eye(3) - diag(x.^2))*mass/12; 120 | end -------------------------------------------------------------------------------- /data/CheetahSysID.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ROAM-Lab-ND/inertia_identification_minimal_examples/fb712026fc9eb587bea5131630f0d74dc5e94cce/data/CheetahSysID.mat -------------------------------------------------------------------------------- /init_path.m: -------------------------------------------------------------------------------- 1 | function init_path() 2 | 3 | for dir = split(path(),':') 4 | if matches(dir,'spatial_v2') 5 | rmpath(dir); 6 | end 7 | end 8 | 9 | spatial_v2_path= [pwd '/spatial_v2_extended']; 10 | err_msg = 'You need to run `git submodule update --init --recursive` to load the submodule first'; 11 | 12 | assert(exist(spatial_v2_path,'dir') > 0,err_msg); 13 | 14 | addpath(genpath(pwd)); 15 | 16 | try 17 | cvx; 18 | catch 19 | assert(1==1,'CVX not detected. Please install it first.') 20 | end 21 | end 22 | 23 | -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | % Need to run this file with its enclosing folder as the working directory 2 | fileInfo = dir(matlab.desktop.editor.getActiveFilename); 3 | cd(fileInfo.folder); 4 | 5 | clear 6 | close all 7 | init_path 8 | %% Load Data 9 | load('CheetahSysID.mat'); 10 | model = Cheetah3LegModel(); 11 | 12 | %% Load Object files 13 | disp("loading CAD mesh files (this may take a while)") 14 | obj = cell(3,1); 15 | obj{1} = readObj('c3_body.obj'); 16 | obj{2} = readObj('c3_upper_link.obj'); % Contains Ab/Ad & Thigh links 17 | obj{3} = readObj('c3_lower_link.obj'); % Shank link 18 | disp("loaded CAD mesh files") 19 | 20 | %% Data Setup 21 | N = length(Y); 22 | n_links = 3; % # of conventional links 23 | n_rotors= 3; % # of rigid-bodies modeled for rotors 24 | n_bodies= n_links+n_rotors; 25 | n_dofs = length(qd{1}); 26 | 27 | % Time vector 28 | dt = 1e-3; 29 | t = (0:(length(q)-1))*dt; 30 | 31 | % Option to regenerate the regressors. 32 | % The regressors are provided in the dataset, 33 | % but here's how you can compute them 34 | regenerate_regressors = 0; 35 | 36 | if regenerate_regressors 37 | Y = {}; 38 | for i =1:N 39 | if mod(i,100) == 0 40 | fprintf('%d / %d Regressors Computed\n',i,N); 41 | end 42 | [Y_i, Yrot_i] = RegressorClassical( model, q{i}, qd{i},qdd{i}); 43 | Y{i,1} = [Y_i Yrot_i]; 44 | end 45 | end 46 | 47 | % Setup Friction Regressor 48 | B = repmat({zeros(n_dofs,n_dofs)}, N,1 ); 49 | Bc = repmat({zeros(n_dofs,n_dofs)}, N,1 ); 50 | for i = 1:N 51 | B{i} = diag( qd{i} ); 52 | Bc{i} = diag( sign( qd{i} ) ); 53 | end 54 | 55 | % Convert cell arrays to matrices 56 | tau_stack = cell2mat(tau); 57 | Y_stack = cell2mat(Y); 58 | tau_mat = cell2mat(tau')'; 59 | B_stack = cell2mat(B); 60 | Bc_stack = cell2mat(Bc); 61 | 62 | Y_total = [Y_stack B_stack Bc_stack]; 63 | 64 | % Optional: Select subset for training data 65 | N_train = N; % Full data set 66 | Y_train = Y_stack(1:n_dofs*N_train,:); 67 | B_train = B_stack(1:n_dofs*N_train,:); 68 | Bc_train = Bc_stack(1:n_dofs*N_train,:); 69 | tau_train = tau_stack(1:n_dofs*N_train,:); 70 | 71 | %% Prior 72 | % The parameter order for each link is: 73 | % m, h_x, h_y, h_z, I_xx, I_yy, I_zz, I_yz, I_xz, I_xy 74 | 75 | [pi_prior, ... % Prior parameters 76 | J_prior, ... % Prior Pseudo-inertias 77 | Q_bound] ... % Bounding Ellipsoids S={ x | [x ; 1]'*Q*[x ; 1] >= 0 } 78 | = Cheetah3_prior_inertia_CAD(); 79 | 80 | figure(1); clf; 81 | color = rand(6,3); 82 | title('Bounding Ellipses'); 83 | Cheetah_bound_visualize(obj, Q_bound, color) 84 | 85 | 86 | %% Conventional System ID with Entropic Regularization 87 | 88 | fprintf('=========== Convex Approach (SDP with LMIs) =========\n'); 89 | 90 | cvx_setup; 91 | 92 | % Clear CVX 93 | cvx_begin 94 | cvx_end 95 | 96 | % Set solver 97 | cvx_solver mosek % <- Can be changed if you don't have it. 98 | 99 | weight_regularization = 1e-1; 100 | use_const_pullback_approx = 0; 101 | force_ellipsoid_density_realizability = 1; 102 | 103 | cvx_begin 104 | variable params(10,n_bodies) % inertial params of links / rotors (units vary) 105 | variable b(n_dofs) % viscous friction coefficient (Nm / (rad/s) 106 | variable bc(n_dofs) % coulomb friction coefficient (Nm) 107 | 108 | variable J(4,4,n_bodies) semidefinite % pseudo-inertia for each body 109 | 110 | expression bregman(n_bodies) % Entropic / bregman divergence for regularization 111 | expression e_train(3*N_train,1); % Training error (Nm) 112 | 113 | % Training error 114 | e_train = Y_train*params(:) + B_train*b + Bc_train*bc - tau_train; 115 | 116 | % Entropic (i.e., bregman) divergence between psuedo-inertias 117 | for i=1:n_bodies 118 | if use_const_pullback_approx == 0 119 | % d( J || J_prior ) 120 | bregman(i) = -log_det( J(:,:,i) ) + log(det(J_prior{i})) ... 121 | + trace(J_prior{i} \ J(:,:,i) ) - 4; 122 | else 123 | % constant pullback approximation 124 | M{i} = pullbackMetric(pi_prior(:,i)); 125 | bregman(i) = 1/2*(params(:,i) - pi_prior(:,i))'*M{i}*(params(:,i) - pi_prior(:,i)); 126 | end 127 | end 128 | 129 | % Objective = Squared 2-norm of residual + Regularization 130 | minimize( 1/2*sum_square_abs(e_train)/length(e_train) ... 131 | + weight_regularization*sum(bregman) ) 132 | 133 | % Only constraints are to set the pseudo-inertias based on params 134 | subject to: 135 | for i=1:n_bodies 136 | J(:,:,i) == inertiaVecToPinertia( params(:,i) ); 137 | if force_ellipsoid_density_realizability 138 | trace( J(:,:,i)*Q_bound{i} ) >= 0; 139 | end 140 | end 141 | cvx_end 142 | 143 | %% 144 | tau_predict_entropic = reshape(Y_stack*params(:) + B_stack*b + Bc_stack*bc,n_dofs,N)'; 145 | plotTorquePredictions(2,'Convex, Entropic Regularized',t,tau_mat, tau_predict_entropic); 146 | 147 | 148 | % Visualize inertia and CAD 149 | color = rand(6,3); 150 | figure(3); clf; 151 | 152 | % Represents the inertial params by solid ellipsoid of uniform density 153 | % Note: This solid need not be within the bounding ellipsoid since 154 | % it represents but one density distribution that realizes the 155 | % parameters 156 | Cheetah_ellipsoid_visualize(obj,params, color); 157 | 158 | grid on; 159 | xlabel('x[m]'); ylabel('y[m]'); zlabel('z[m]'); 160 | title('Inertia Ellipsoids (Convex, Entropic Regularized)'); 161 | 162 | 163 | %% Unconstrained System ID with Entropic Regularization 164 | 165 | fprintf('=========== Log Cholesky Approach (unconstrained) =========\n'); 166 | 167 | options = optimoptions('fminunc'); 168 | options.Algorithm = 'trust-region'; 169 | options.HessianFcn = 'objective'; 170 | options.SpecifyObjectiveGradient = true; 171 | options.StepTolerance = 1.0000e-6; 172 | options.MaxFunctionEvaluations = 132000; 173 | options.MaxIterations = 2000; 174 | options.Display = 'iter'; 175 | 176 | data.Y = [Y_train B_train Bc_train]; 177 | data.tau = tau_train; 178 | data.prior = pi_prior; 179 | data.gamma = weight_regularization; 180 | 181 | x_init = zeros(66,1); 182 | 183 | % Main optimization step 184 | cholParams = fminunc( @(x) objective(x, data), x_init,options ); 185 | 186 | % System Param Map converts from cholesky Parameters to conventional params 187 | piParams = systemParamMap( cholParams, data.prior ); 188 | 189 | 190 | tau_predic_logchol = reshape(Y_total*piParams, n_dofs,N)'; 191 | 192 | plotTorquePredictions(4,'Log Cholesky, Entropic Regularized',t,tau_mat, tau_predic_logchol); 193 | 194 | 195 | % Visualize inertia and CAD 196 | color = rand(6,3); 197 | figure(5); clf; 198 | Cheetah_ellipsoid_visualize(obj,reshape(piParams(1:60),10,6), color); 199 | grid on; 200 | xlabel('x[m]'); ylabel('y[m]'); zlabel('z[m]'); 201 | title('Inertia Ellipsoids (Log Cholesky, Entropic Regularized)'); 202 | 203 | 204 | 205 | %% Kinematics Plotting 206 | q_mat = cell2mat(q')'; 207 | figure(6); clf; 208 | subplot(311); 209 | plot(t,q_mat(:,1)) 210 | ylabel('Ab/Ad angle (rad)'); 211 | title('Leg Kinematics'); 212 | subplot(312); 213 | plot(t,q_mat(:,2)) 214 | ylabel('Hip angle (rad)'); 215 | subplot(313); 216 | plot(t,q_mat(:,3)) 217 | ylabel('Knee angle (rad)'); 218 | xlabel('Time (s)'); 219 | 220 | 221 | %% Helpers 222 | function plotTorquePredictions(figNum, name, t, tau_actual , tau_predict) 223 | tau_rms = rms(tau_actual - tau_predict); 224 | figure(figNum) 225 | clf 226 | subplot(311) 227 | plot(t,tau_actual(:,1)); hold on; 228 | plot(t,tau_predict(:,1),'r','LineWidth',1.5 ) 229 | ylabel('Ab/Ad Torque (Nm)'); 230 | rms_tag = sprintf('(%s) [RMS=%.2f, %.2f, %.2f (Nm)]',name, tau_rms(1), tau_rms(2), tau_rms(3)); 231 | title(['Torque Predictions ' rms_tag]); 232 | legend('Measured','Predicted') 233 | 234 | subplot(312) 235 | plot(t,tau_actual(:,2)); hold on; 236 | plot(t,tau_predict(:,2),'r','LineWidth',1.5) 237 | ylabel('Hip Torque (Nm)'); 238 | 239 | subplot(313) 240 | plot(t,tau_actual(:,3)); hold on; 241 | plot(t,tau_predict(:,3),'r','LineWidth',1.5) 242 | ylabel('Knee Torque (Nm)'); 243 | xlabel('Time (s)'); 244 | end 245 | 246 | % All of this from here on out is basically just to get analytical 247 | % jacobians / hessians of the objective. 248 | 249 | function [f, J, H] = objective(x, data) 250 | if nargout == 1 251 | f = regularizedLeastSquares( systemParamMap(x,data.prior), data); 252 | elseif nargout == 1 253 | % We have a map g: Log Cholesky -> standard parameters 254 | % We also have a objective function f : standard parameters -> Real 255 | % It is easy to get the jacobian and hessian of f and g. 256 | % The function used below gets the jacobian and hessian of their 257 | % composition. 258 | [f, J] = composedJacobianAndHessian( @(x) regularizedLeastSquares(x, data) ... 259 | , @(x) systemParamMap(x, data.prior) , x); 260 | else 261 | [f, J, H] = composedJacobianAndHessian( @(x) regularizedLeastSquares(x, data) ... 262 | , @(x) systemParamMap(x, data.prior) , x); 263 | end 264 | end 265 | -------------------------------------------------------------------------------- /utiltiy/QtoSemiAxes.m: -------------------------------------------------------------------------------- 1 | function [Axes, center] = QtoSemiAxes(Q) 2 | [Axes, D] = eig(-Q(1:3,1:3)); 3 | Axes = Axes/sqrt(D); 4 | Qc = Q(1:3,4); 5 | Q = inv(-Q(1:3,1:3)); 6 | center = Q*Qc; 7 | end 8 | 9 | -------------------------------------------------------------------------------- /utiltiy/SemiAxesToQ.m: -------------------------------------------------------------------------------- 1 | function Q = SemiAxesToQ(Axes,center) 2 | if nargin == 1 3 | center = [0;0;0]; 4 | end 5 | 6 | Q = inv(Axes*Axes'); 7 | 8 | Qc = Q*center; 9 | Q = [ -Q Qc ; Qc' 1-center'*Qc]; 10 | end 11 | 12 | -------------------------------------------------------------------------------- /utiltiy/composedJacobianAndHessian.m: -------------------------------------------------------------------------------- 1 | function [f, J, H] = composedJacobianAndHessian( f1, f2, x) 2 | % Computes the jacobian J and hess H of the function composition 3 | % f1(f2(x)) at the point x, by using the jacobian and hessian of f1 and 4 | % f2 5 | 6 | if nargout == 1 7 | [y] = f2(x); 8 | [f] = f1(y); 9 | elseif nargout == 2 10 | [y, J2] = f2(x); 11 | [f, J1] = f1(y); 12 | J = J1*J2; 13 | else 14 | [y, J2, H2] = f2(x); 15 | [f, J1, H1] = f1(y); 16 | 17 | n = length(x); 18 | 19 | % Compute the pesky tensor contraction terms 20 | H_pesky = J1*reshape( H2, [size(H2,1) n*n]); 21 | H_pesky = reshape(H_pesky, [n n]); 22 | 23 | J = J1*J2; 24 | H = J2'*H1*J2 + H_pesky; 25 | end 26 | end -------------------------------------------------------------------------------- /utiltiy/logCholeskyVecToParams.m: -------------------------------------------------------------------------------- 1 | function [pi, Jac, Hess]= logCholeskyVecToParams(x, pi0) 2 | % Converts log-cholesky params to standard inertial params 3 | % see: Smooth Parameterization of Rigid-Body Inertia by (Rucker and 4 | % Wensing) https://ieeexplore.ieee.org/document/9690029 5 | 6 | d1 = x(1); 7 | d2 = x(2); 8 | d3 = x(3); 9 | s12 = x(4); 10 | s13 = x(5); 11 | s23 = x(6); 12 | t1 = x(7); 13 | t2 = x(8); 14 | t3 = x(9); 15 | a = x(10); 16 | 17 | U = exp(a)*[exp(d1) s12 s13, t1;0 exp(d2) s23, t2;0 0 exp(d3), t3; 0,0,0,1]; 18 | if nargin == 1 19 | J0 = eye(4); 20 | else 21 | J0 = inertiaVecToPinertia(pi0); 22 | end 23 | 24 | J = U*J0*U.'; 25 | 26 | pi = pinertiaToVec(J); 27 | 28 | if nargout > 1 29 | if nargin == 1 30 | Jac = singleBodyParamJacobian(x); 31 | else 32 | Jac = singleBodyParamJacobian(x, pi0); 33 | end 34 | end 35 | if nargout > 2 36 | if nargin == 1 37 | Hess = singleBodyParamHessian(x); 38 | else 39 | Hess = singleBodyParamHessian(x, pi0); 40 | end 41 | end 42 | end 43 | 44 | -------------------------------------------------------------------------------- /utiltiy/regularizedLeastSquares.m: -------------------------------------------------------------------------------- 1 | function [f, J, H] = regularizedLeastSquares( x, data) 2 | % Computes the objective for an entropically regularized least-square 3 | % problem 4 | % The input x represents the standard params. 5 | % Note: The indicies in this function are specific to the system used 6 | 7 | Y = data.Y; 8 | e = Y*x - data.tau ; 9 | 10 | if nargout > 1 11 | J = e.'*Y / length(e) ; 12 | end 13 | if nargout > 2 14 | H = data.Y'*data.Y / length(e) ; 15 | end 16 | 17 | f = 1/2*e.'*e./length(e); 18 | pi = reshape(x(1:60), 10,6); % Specific to this system 19 | 20 | for i = 1:6 21 | i1 = (i-1)*10+1; 22 | i2 = i*10; 23 | inds = i1:i2; 24 | if nargout == 1 25 | f = f+ data.gamma*entropicDivergence( pi(:,i), data.prior(:,i) ); 26 | elseif nargout == 2 27 | [f_reg, J_reg] = entropicDivergence( pi(:,i), data.prior(:,i) ); 28 | 29 | f = f + data.gamma*f_reg; 30 | J(inds) = J(inds) + data.gamma*J_reg.'; 31 | else 32 | [f_reg, J_reg, H_reg] = entropicDivergence( pi(:,i), data.prior(:,i) ); 33 | f = f + data.gamma*f_reg; 34 | J(inds) = J(inds) + data.gamma*J_reg.'; 35 | H(inds, inds) = H(inds, inds) + data.gamma*H_reg; 36 | end 37 | end 38 | end 39 | -------------------------------------------------------------------------------- /utiltiy/rms.m: -------------------------------------------------------------------------------- 1 | function RMS= rms(varargin) 2 | % 3 | % Written by Phillip M. Feldman March 31, 2006 4 | % 5 | % rms computes the root-mean-square (RMS) of values supplied as a 6 | % vector, matrix, or list of discrete values (scalars). If the input is 7 | % a matrix, rms returns a row vector containing the RMS of each column. 8 | % David Feldman proposed the following simpler function definition: 9 | % 10 | % RMS = sqrt(mean([varargin{:}].^2)) 11 | % 12 | % With this definition, the function accepts ([1,2],[3,4]) as input, 13 | % producing 2.7386 (this is the same result that one would get with 14 | % input of (1,2,3,4). I'm not sure how the function should behave for 15 | % input of ([1,2],[3,4]). Probably it should produce the vector 16 | % [rms(1,3) rms(2,4)]. For the moment, however, my code simply produces 17 | % an error message when the input is a list that contains one or more 18 | % non-scalars. 19 | if (nargin == 0) 20 | error('Missing input.'); 21 | end 22 | % Section 1: Restructure input to create x vector. 23 | if (nargin == 1) 24 | x= varargin{1}; 25 | else 26 | for i= 1 : size(varargin,2) 27 | if (prod(size(varargin{i})) ~= 1) 28 | error(['When input is provided as a list, ' ... 29 | 'list elements must be scalar.']); 30 | end 31 | x(i)= varargin{i}; 32 | end 33 | end 34 | % Section 2: Compute RMS value of x. 35 | RMS= sqrt (mean (x .^2) ); 36 | end -------------------------------------------------------------------------------- /utiltiy/splitParams.m: -------------------------------------------------------------------------------- 1 | function [pi, b , bc] = splitParams(x) 2 | % Breaks out parameters into those of rigid bodies and for 3 | % viscous/coulomb friction 4 | % Note: The indicies in this function are specific to the system used 5 | 6 | pi = x(1:60); % Rigid body params 7 | b = x(61:63); % viscous friction 8 | bc = x(64:66); % coulomb friction 9 | end 10 | -------------------------------------------------------------------------------- /utiltiy/systemParamHessian.m: -------------------------------------------------------------------------------- 1 | function H = systemParamHessian(x, x0) 2 | % Hessian tensor for the systemParamMap function 3 | % The indicies in this function are specific to the system used 4 | 5 | [pi, b , bc] = splitParams(x); 6 | pi = reshape(pi,10,6); 7 | if nargin == 2 8 | pi0 = reshape( x0(1:60), 10,6); 9 | end 10 | H = zeros(66,66,66); 11 | for i = 1:6 12 | i1 = (i-1)*10+1; 13 | i2 = i*10; 14 | inds = i1:i2; 15 | if nargin == 1 16 | prior = pinertiaToVec(eye(4)); 17 | else 18 | prior = pi0(:,i); 19 | end 20 | H(inds, inds, inds) = singleBodyParamHessian( pi(:,i), prior); 21 | end 22 | for i = 61:66 23 | H(i,i,i) = exp( x(i) ); 24 | end 25 | end 26 | 27 | -------------------------------------------------------------------------------- /utiltiy/systemParamJacobian.m: -------------------------------------------------------------------------------- 1 | function J = systemParamJacobian(x, x0) 2 | % Jacobian of the systemParamMap function 3 | % The indicies in this function are specific to the system used 4 | 5 | pi = reshape(x(1:60),10,6); 6 | if nargin == 2 7 | pi0 = reshape( x0(1:60), 10,6); 8 | end 9 | 10 | for i = 1:6 11 | i1 = (i-1)*10+1; 12 | i2 = i*10; 13 | inds = i1:i2; 14 | 15 | if nargin == 1 16 | prior = pinertiaToVec(eye(4)); 17 | else 18 | prior = pi0(:,i); 19 | end 20 | J( inds, inds ) = singleBodyParamJacobian( pi(:,i), prior); 21 | end 22 | J(61:66, 61:66) = diag( exp(x(61:end)) ); 23 | end 24 | 25 | -------------------------------------------------------------------------------- /utiltiy/systemParamMap.m: -------------------------------------------------------------------------------- 1 | function [y, J, H] = systemParamMap(x, x0) 2 | % This function maps from log cholesky params to conventional params 3 | % The indicies in this function are specific to the system used 4 | 5 | [pi, b , bc] = splitParams(x); 6 | pi = reshape(pi,10,6); 7 | 8 | if nargin == 2 9 | pi0 = reshape( x0(1:60), 10,6); 10 | end 11 | 12 | for i = 1:6 13 | if nargin == 1 14 | y(:,i) = logCholeskyVecToParams( pi(:,i) ); 15 | else 16 | y(:,i) = logCholeskyVecToParams( pi(:,i), pi0(:,i) ); 17 | end 18 | end 19 | y = y(:); 20 | 21 | % Friction coefficients 22 | for i = 61:66 23 | y(i) = exp( x(i) ); 24 | end 25 | 26 | if nargout > 1 27 | J = systemParamJacobian(x, x0); 28 | end 29 | if nargout >2 30 | H = systemParamHessian(x, x0); 31 | end 32 | 33 | end -------------------------------------------------------------------------------- /vis/Cheetah3_prior_inertia_CAD.m: -------------------------------------------------------------------------------- 1 | function [pi_prior, P, Q_bound_ellipse] = Cheetah3_prior_inertia_CAD() 2 | 3 | [model, graphics] = Cheetah3LegModel(); 4 | NB = model.NB; 5 | 6 | pi_prior = zeros(10, NB*2); 7 | Q_bound_ellipse = cell(NB*2,1); 8 | P =cell(NB*2,1); % Pseudo inertia matrix for body i 9 | Qc=cell(NB*2,1); % 3x3 Matrix which provides a bounding ellipse for the full rigid body 10 | c =cell(NB*2,1); % Center of Bounding Ellipsoid 11 | 12 | % Prior Params 13 | for i = 1 : model.NB 14 | pi_prior(:,i) = inertiaMatToVec(model.I{i}); 15 | pi_prior(:,i+3) = inertiaMatToVec(model.I_rotor{i}); 16 | end 17 | 18 | % Loop through one body at a time. 19 | for i = 1:2*NB 20 | P{i} = inertiaVecToPinertia(pi_prior(:,i)); 21 | 22 | assert( min(eig(P{i})) > 0 ) 23 | 24 | if i <= NB 25 | boundSemiAxisLengths = graphics{i}.boundAxes; 26 | boundCenter = graphics{i}.boundCenter; 27 | else 28 | boundSemiAxisLengths = graphics{i-NB}.boundAxesRot; 29 | boundCenter = graphics{i-NB}.boundCenterRot; 30 | end 31 | 32 | Q_bound_ellipse{i} = SemiAxesToQ( diag(boundSemiAxisLengths) ,boundCenter); 33 | 34 | % If the parameters a{i} are realizable with density on the bounding 35 | % ellipse, this trace must be positive 36 | tQP = trace(Q_bound_ellipse{i}*P{i}); 37 | assert( tQP > 0 ) 38 | end 39 | end 40 | -------------------------------------------------------------------------------- /vis/Cheetah_bound_visualize.m: -------------------------------------------------------------------------------- 1 | function Cheetah_bound_visualize(obj, Q, color_) 2 | q =[pi/6;pi/5;-1.0*2*pi/3]; 3 | 4 | [model, ~] = Cheetah3LegModel(); 5 | NB = model.NB; 6 | p = model.parent; 7 | 8 | color = color_(1:NB,:); 9 | color_rot = color_(NB+1:end,:); 10 | 11 | q_CAD = q + [0;pi;pi]; % Different joint angles to account for different zero config in CAD 12 | 13 | hold on; 14 | for i = 1:NB 15 | [ XJ, ~] = jcalc( model.jtype{i}, q(i) ); 16 | [ XJrot, ~] = jcalc( model.jtype{i}, q(i)*model.gr{i} ); 17 | [ XJ_CAD, ~] = jcalc( model.jtype{i}, q_CAD(i) ); 18 | 19 | if i == 2 20 | % Account for offset CAD origin for thigh link 21 | XJ_CAD = XJ_CAD * [eye(3,3),zeros(3,3);skew([0;-0.045;0]),eye(3,3)]; 22 | end 23 | 24 | Xup{i} = XJ * model.Xtree{i}; 25 | Xrot{i} = XJrot * model.Xrotor{i}; 26 | Xup_CAD{i} = XJ_CAD * model.Xtree{i}; 27 | 28 | if model.parent(i) == 0 29 | X0{i} = Xup{i}; 30 | X0rot{i} = Xrot{i}; 31 | X0_CAD{i} = Xup_CAD{i}; 32 | 33 | else 34 | X0{i} = Xup{i} * X0{p(i)}; 35 | X0rot{i} = Xrot{i} * X0{p(i)}; 36 | X0_CAD{i} = Xup_CAD{i} * X0{p(i)}; 37 | 38 | end 39 | 40 | T = inv( AdjointToSE3( X0{i} ) ); 41 | Trot = inv( AdjointToSE3( X0rot{i} ) ); 42 | 43 | plot_bounding_ellipsoid(T, Q{i}, .45, color(:,i)) 44 | plot_bounding_ellipsoid(Trot, Q{i+NB}, .45, color_rot(:,i)) 45 | 46 | T = inv( AdjointToSE3( X0_CAD{i} ) ); 47 | 48 | if i>1 49 | V = (T(1:3,1:3)*obj{i}.v' + T(1:3,4) * ones(1,size(obj{i}.v,1)))'; 50 | F = obj{i}.f.v; 51 | patch('vertices', V, 'faces', F,'LineStyle','none','FaceAlpha',.15); 52 | end 53 | end 54 | axis equal; 55 | view([-140 26]); 56 | draw_SE3(eye(4,4)); 57 | end -------------------------------------------------------------------------------- /vis/Cheetah_ellipsoid_visualize.m: -------------------------------------------------------------------------------- 1 | function Cheetah_ellipsoid_visualize(obj, Phi, color_) 2 | q =[pi/6;pi/5;-1.0*2*pi/3]; 3 | 4 | [model, ~] = Cheetah3LegModel(); 5 | NB = model.NB; 6 | p = model.parent; 7 | 8 | for i = 1 : NB 9 | I{i} = inertiaVecToMat(Phi(:,i)); 10 | Irot{i} = inertiaVecToMat(Phi(:,NB+i)); 11 | end 12 | 13 | color = color_(1:NB,:); 14 | color_rot = color_(NB+1:end,:); 15 | 16 | q_CAD = q + [0;pi;pi]; % Different joint angles to account for different zero config in CAD 17 | 18 | hold on; 19 | for i = 1:NB 20 | [ XJ, ~] = jcalc( model.jtype{i}, q(i) ); 21 | [ XJrot, ~] = jcalc( model.jtype{i}, q(i)*model.gr{i} ); 22 | [ XJ_CAD, ~] = jcalc( model.jtype{i}, q_CAD(i) ); 23 | 24 | if i == 2 25 | % Account for offset CAD origin for thigh link 26 | XJ_CAD = XJ_CAD * [eye(3,3),zeros(3,3);skew([0;-0.045;0]),eye(3,3)]; 27 | end 28 | 29 | Xup{i} = XJ * model.Xtree{i}; 30 | Xrot{i} = XJrot * model.Xrotor{i}; 31 | Xup_CAD{i} = XJ_CAD * model.Xtree{i}; 32 | 33 | if model.parent(i) == 0 34 | X0{i} = Xup{i}; 35 | X0rot{i} = Xrot{i}; 36 | X0_CAD{i} = Xup_CAD{i}; 37 | 38 | else 39 | X0{i} = Xup{i} * X0{p(i)}; 40 | X0rot{i} = Xrot{i} * X0{p(i)}; 41 | X0_CAD{i} = Xup_CAD{i} * X0{p(i)}; 42 | 43 | end 44 | 45 | T = inv( AdjointToSE3( X0{i} ) ); 46 | Trot = inv( AdjointToSE3( X0rot{i} ) ); 47 | 48 | plot_inertiatensor(T, I{i}, 0.45, color(i,:)); 49 | plot_inertiatensor(Trot, Irot{i}, 0.45, color_rot(i,:)); 50 | 51 | T = inv( AdjointToSE3( X0_CAD{i} ) ); 52 | 53 | if i>1 54 | V = (T(1:3,1:3)*obj{i}.v' + T(1:3,4) * ones(1,size(obj{i}.v,1)))'; 55 | F = obj{i}.f.v; 56 | patch('vertices', V, 'faces', F,'LineStyle','none','FaceAlpha',.15); 57 | end 58 | end 59 | axis equal; 60 | view([-140 26]); 61 | draw_SE3(eye(4,4)); 62 | end -------------------------------------------------------------------------------- /vis/draw_SE3.m: -------------------------------------------------------------------------------- 1 | function draw_SE3(T,string_color) 2 | p=T(1:3,4); 3 | ax=p+T(1:3,1)*0.12; 4 | ay=p+T(1:3,2)*0.12; 5 | az=p+T(1:3,3)*0.12; 6 | hold on; 7 | if nargin < 2 8 | plot3([p(1),ax(1)], [p(2),ax(2)], [p(3),ax(3)],'black','LineWidth', 1.5); 9 | plot3([p(1),ay(1)], [p(2),ay(2)], [p(3),ay(3)],'black','LineWidth', 1.5); 10 | plot3([p(1),az(1)], [p(2),az(2)], [p(3),az(3)],'black','LineWidth', 1.5); 11 | else 12 | plot3([p(1),ax(1)], [p(2),ax(2)], [p(3),ax(3)],string_color,'LineWidth', 1.5); 13 | plot3([p(1),ay(1)], [p(2),ay(2)], [p(3),ay(3)],string_color,'LineWidth', 1.5); 14 | plot3([p(1),az(1)], [p(2),az(2)], [p(3),az(3)],string_color,'LineWidth', 1.5); 15 | end 16 | % axis([-1/2 1/2 -1/2 1.5/2 0 1.2]); 17 | end -------------------------------------------------------------------------------- /vis/plot_bounding_ellipsoid.m: -------------------------------------------------------------------------------- 1 | function plot_bounding_ellipsoid(T_0i, Q, trans, color) 2 | 3 | [Axes, center] = QtoSemiAxes(Q); 4 | Q = Axes*Axes'; 5 | 6 | [R,S] = eig(Q); 7 | if det(R)<0 8 | R_temp = R; S_temp = S; 9 | R(:,1) = R_temp(:,2); 10 | R(:,2) = R_temp(:,1); 11 | S(1,1) = S_temp(2,2); 12 | S(2,2) = S_temp(1,1); 13 | end 14 | 15 | T = [R, zeros(3,1);0,0,0,1]; 16 | T(1:3,4) = center; 17 | 18 | T = T_0i*T; 19 | 20 | S = sqrt(S); 21 | 22 | a = S(1,1); 23 | b = S(2,2); 24 | c = S(3,3); 25 | 26 | scale = 1; 27 | [xc,yc,zc] = ellipsoid(0,0,0,a*scale,b*scale,c*scale,200); 28 | 29 | % rotate data with orientation matrix U and center M 30 | RR = T(1:3,1:3); M = T(1:3,4); 31 | 32 | a = kron(RR(:,1),xc); b = kron(RR(:,2),yc); c = kron(RR(:,3),zc); 33 | data = a+b+c; n = size(data,2); 34 | x = data(1:n,:)+M(1); y = data(n+1:2*n,:)+M(2); z = data(2*n+1:end,:)+M(3); 35 | 36 | sc = surf(x,y,z,'EdgeColor','none','FaceColor',color,'FaceAlpha',trans); 37 | -------------------------------------------------------------------------------- /vis/plot_inertiatensor.m: -------------------------------------------------------------------------------- 1 | function sc = plot_inertiatensor(T_sb_i, G_b_i, trans, color, varargin) 2 | n_parts = size(G_b_i,3); 3 | 4 | for i=[1:n_parts] 5 | % [R,S,V] = svd(G_sbar_init{i}(1:3,1:3)); 6 | % a = sqrt(5*(-S(1,1)+S(2,2)+S(3,3))/(2*G_sbar_init{i}(4,4))); 7 | % b = sqrt(5*(S(1,1)-S(2,2)+S(3,3))/(2*G_sbar_init{i}(4,4))); 8 | % c = sqrt(5*(S(1,1)+S(2,2)-S(3,3))/(2*G_sbar_init{i}(4,4))); 9 | T_bc = [eye(3,3), skew(G_b_i(1:3,4:6,i))./G_b_i(4,4,i);0,0,0,1]; 10 | G_c = AdjointMatrix(T_bc)'*G_b_i(:,:,i)*AdjointMatrix(T_bc); 11 | % [R,S,V] = svd(G_b(1:3,1:3)); 12 | [R,S,V] = eig(G_c(1:3,1:3)); 13 | if det(R)<0 14 | R_temp = R; S_temp = S; 15 | R(:,1) = R_temp(:,2); 16 | R(:,2) = R_temp(:,1); 17 | S(1,1) = S_temp(2,2); 18 | S(2,2) = S_temp(1,1); 19 | end 20 | % 0.5*eye(3,3)*trace(S) - S 21 | T = [R, zeros(3,1);0,0,0,1]; 22 | T(1:3,4) = skew(G_b_i(1:3,4:6,i))./G_b_i(4,4,i); 23 | T = T_sb_i(:,:,i)*T; 24 | a = sqrt(5*(-S(1,1)+S(2,2)+S(3,3))/(2*(G_b_i(4,4,i)))); 25 | if ~isreal(a) 26 | a = 0 27 | end 28 | b = sqrt(5*(S(1,1)-S(2,2)+S(3,3))/(2*(G_b_i(4,4,i)))); 29 | if ~isreal(b) 30 | b = 0 31 | end 32 | c = sqrt(5*(S(1,1)+S(2,2)-S(3,3))/(2*(G_b_i(4,4,i)))); 33 | if ~isreal(c) 34 | c = 0 35 | end 36 | % T(1:3,1:3) = T(1:3,1:3)*[a,0,0;0,b,0;0,0,c]; 37 | % draw_SE3(T); 38 | % transl = T(1:3,1:3)'*T(1:3,4); 39 | % density = G_b_i(4,4,i)/((a+0.001)*(b+0.001)*(c+0.001)*2e3); 40 | scale = 1; 41 | [xc,yc,zc] = ellipsoid(0,0,0,a*scale,b*scale,c*scale,200); 42 | 43 | % rotate data with orientation matrix U and center M 44 | RR = T(1:3,1:3); M = T(1:3,4); 45 | a = kron(RR(:,1),xc); b = kron(RR(:,2),yc); c = kron(RR(:,3),zc); 46 | data = a+b+c; n = size(data,2); 47 | x = data(1:n,:)+M(1); y = data(n+1:2*n,:)+M(2); z = data(2*n+1:end,:)+M(3); 48 | % scatter3(M(1),M(2),M(3), 'MarkerEdgeColor',color(i,:),'MarkerFaceColor',color(i,:)); 49 | % now plot the rotated ellipse 50 | % color_z = ones(size(z))*color; 51 | 52 | % text(M(1),M(2),M(3),[ num2str(G_b_i(4,4,i)) 'kg'],'HorizontalAlignment','left','FontSize',8); 53 | 54 | if nargin > 4 55 | sc_in = varargin{1}; 56 | set(sc_in,'XData' ,x); 57 | set(sc_in,'YData' ,y); 58 | set(sc_in,'ZData' ,z); 59 | else 60 | sc = surf(x,y,z,'EdgeColor','none','FaceColor',color(i,:),'FaceAlpha',trans); 61 | end 62 | 63 | 64 | end 65 | % shading interp; 66 | % lighting phong; 67 | % camlight; 68 | axis equal; 69 | % drawnow; %hold off; 70 | % draw_SE3(T_sb_i); 71 | end 72 | 73 | function Ad_T = AdjointMatrix(T) 74 | R = T(1:3,1:3); 75 | p = T(1:3,4); 76 | Ad_T = [R, zeros(3,3);skew(p)*R, R]; 77 | end -------------------------------------------------------------------------------- /vis/readObj.m: -------------------------------------------------------------------------------- 1 | function obj = readObj(fname) 2 | % 3 | % obj = readObj(fname) 4 | % 5 | % This function parses wavefront object data 6 | % It reads the mesh vertices, texture coordinates, normal coordinates 7 | % and face definitions(grouped by number of vertices) in a .obj file 8 | % 9 | % 10 | % INPUT: fname - wavefront object file full path 11 | % 12 | % OUTPUT: obj.v - mesh vertices 13 | % : obj.vt - texture coordinates 14 | % : obj.vn - normal coordinates 15 | % : obj.f - face definition assuming faces are made of of 3 vertices 16 | % 17 | % Bernard Abayowa, Tec^Edge 18 | % 11/8/07 19 | 20 | % set up field types 21 | v = []; vt = []; vn = []; f.v = []; f.vt = []; f.vn = []; 22 | 23 | fid = fopen(fname); 24 | 25 | % parse .obj file 26 | while 1 27 | tline = fgetl(fid); 28 | if ~ischar(tline), break, end % exit at end of file 29 | ln = sscanf(tline,'%s',1); % line type 30 | %disp(ln) 31 | switch ln 32 | case 'v' % mesh vertexs 33 | v = [v; sscanf(tline(2:end),'%f')']; 34 | case 'vt' % texture coordinate 35 | vt = [vt; sscanf(tline(3:end),'%f')']; 36 | case 'vn' % normal coordinate 37 | vn = [vn; sscanf(tline(3:end),'%f')']; 38 | case 'f' % face definition 39 | fv = []; fvt = []; fvn = []; 40 | str = textscan(tline(2:end),'%s'); str = str{1}; 41 | 42 | nf = length(findstr(str{1},'/')); % number of fields with this face vertices 43 | 44 | 45 | [tok str] = strtok(str,'//'); % vertex only 46 | for k = 1:length(tok) fv = [fv str2num(tok{k})]; end 47 | 48 | if (nf > 0) 49 | [tok str] = strtok(str,'//'); % add texture coordinates 50 | for k = 1:length(tok) fvt = [fvt str2num(tok{k})]; end 51 | end 52 | if (nf > 1) 53 | [tok str] = strtok(str,'//'); % add normal coordinates 54 | for k = 1:length(tok) fvn = [fvn str2num(tok{k})]; end 55 | end 56 | f.v = [f.v; fv]; f.vt = [f.vt; fvt]; f.vn = [f.vn; fvn]; 57 | end 58 | end 59 | fclose(fid); 60 | 61 | % set up matlab object 62 | obj.v = v; obj.vt = vt; obj.vn = vn; obj.f = f; 63 | --------------------------------------------------------------------------------