Add test tsid-formulation test for contact points. Adding quadruped urdf model

models/quadruped/urdf/cube.obj

+o Box
+v 0.5 0.5 0.5
+v 0.5 0.5 -0.5
+v 0.5 -0.5 0.5
+v 0.5 -0.5 -0.5
+v -0.5 0.5 -0.5
+v -0.5 0.5 0.5
+v -0.5 -0.5 -0.5
+v -0.5 -0.5 0.5
+v -0.5 0.5 -0.5
+v 0.5 0.5 -0.5
+v -0.5 0.5 0.5
+v 0.5 0.5 0.5
+v -0.5 -0.5 0.5
+v 0.5 -0.5 0.5
+v -0.5 -0.5 -0.5
+v 0.5 -0.5 -0.5
+v -0.5 0.5 0.5
+v 0.5 0.5 0.5
+v -0.5 -0.5 0.5
+v 0.5 -0.5 0.5
+v 0.5 0.5 -0.5
+v -0.5 0.5 -0.5
+v 0.5 -0.5 -0.5
+v -0.5 -0.5 -0.5
+vt 0 1
+vt 1 1
+vt 0 0
+vt 1 0
+vt 0 1
+vt 1 1
+vt 0 0
+vt 1 0
+vt 0 1
+vt 1 1
+vt 0 0
+vt 1 0
+vt 0 1
+vt 1 1
+vt 0 0
+vt 1 0
+vt 0 1
+vt 1 1
+vt 0 0
+vt 1 0
+vt 0 1
+vt 1 1
+vt 0 0
+vt 1 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+f 1/1/1 3/3/3 2/2/2
+f 3/3/3 4/4/4 2/2/2
+f 5/5/5 7/7/7 6/6/6
+f 7/7/7 8/8/8 6/6/6
+f 9/9/9 11/11/11 10/10/10
+f 11/11/11 12/12/12 10/10/10
+f 13/13/13 15/15/15 14/14/14
+f 15/15/15 16/16/16 14/14/14
+f 17/17/17 19/19/19 18/18/18
+f 19/19/19 20/20/20 18/18/18
+f 21/21/21 23/23/23 22/22/22
+f 23/23/23 24/24/24 22/22/22

models/quadruped/urdf/cylinder.obj

+o Cylinder
+v 0 -0.998248581950179 0.5034876052441187
+v 0 -1.0017392333654023 -0.4965063024136719
+v 0.7071067690849304 -0.705357135440903 0.5024652170731156
+v 0 -1.0017392333654023 -0.4965063024136719
+v 0.7071067690849304 -0.7088477868561263 -0.4975286905846749
+v 0.7071067690849304 -0.705357135440903 0.5024652170731156
+v 0.7071067690849304 -0.705357135440903 0.5024652170731156
+v 0.7071067690849304 -0.7088477868561263 -0.4975286905846749
+v 1 0.00174532570761165 0.4999969538288953
+v 0.7071067690849304 -0.7088477868561263 -0.4975286905846749
+v 1 -0.0017453257076117724 -0.4999969538288953
+v 1 0.00174532570761165 0.4999969538288953
+v 1 0.00174532570761165 0.4999969538288953
+v 1 -0.0017453257076117724 -0.4999969538288953
+v 0.7071067690849304 0.7088477868561263 0.4975286905846749
+v 1 -0.0017453257076117724 -0.4999969538288953
+v 0.7071067690849304 0.705357135440903 -0.5024652170731156
+v 0.7071067690849304 0.7088477868561263 0.4975286905846749
+v 0.7071067690849304 0.7088477868561263 0.4975286905846749
+v 0.7071067690849304 0.705357135440903 -0.5024652170731156
+v 1.2246468525851679e-16 1.0017392333654023 0.4965063024136719
+v 0.7071067690849304 0.705357135440903 -0.5024652170731156
+v 1.2246468525851679e-16 0.998248581950179 -0.5034876052441187
+v 1.2246468525851679e-16 1.0017392333654023 0.4965063024136719
+v 1.2246468525851679e-16 1.0017392333654023 0.4965063024136719
+v 1.2246468525851679e-16 0.998248581950179 -0.5034876052441187
+v -0.7071067690849304 0.7088477868561263 0.4975286905846749
+v 1.2246468525851679e-16 0.998248581950179 -0.5034876052441187
+v -0.7071067690849304 0.705357135440903 -0.5024652170731156
+v -0.7071067690849304 0.7088477868561263 0.4975286905846749
+v -0.7071067690849304 0.7088477868561263 0.4975286905846749
+v -0.7071067690849304 0.705357135440903 -0.5024652170731156
+v -1 0.001745325707611895 0.4999969538288953
+v -0.7071067690849304 0.705357135440903 -0.5024652170731156
+v -1 -0.0017453257076115276 -0.4999969538288953
+v -1 0.001745325707611895 0.4999969538288953
+v -1 0.001745325707611895 0.4999969538288953
+v -1 -0.0017453257076115276 -0.4999969538288953
+v -0.7071067690849304 -0.705357135440903 0.5024652170731156
+v -1 -0.0017453257076115276 -0.4999969538288953
+v -0.7071067690849304 -0.7088477868561263 -0.4975286905846749
+v -0.7071067690849304 -0.705357135440903 0.5024652170731156
+v -0.7071067690849304 -0.705357135440903 0.5024652170731156
+v -0.7071067690849304 -0.7088477868561263 -0.4975286905846749
+v -2.4492937051703357e-16 -0.998248581950179 0.5034876052441187
+v -0.7071067690849304 -0.7088477868561263 -0.4975286905846749
+v -2.4492937051703357e-16 -1.0017392333654023 -0.4965063024136719
+v -2.4492937051703357e-16 -0.998248581950179 0.5034876052441187
+v 0 -0.998248581950179 0.5034876052441187
+v 0.7071067690849304 -0.705357135440903 0.5024652170731156
+v 0 0.0017453257076117112 0.4999969538288953
+v 0.7071067690849304 -0.705357135440903 0.5024652170731156
+v 1 0.00174532570761165 0.4999969538288953
+v 0 0.0017453257076117112 0.4999969538288953
+v 1 0.00174532570761165 0.4999969538288953
+v 0.7071067690849304 0.7088477868561263 0.4975286905846749
+v 0 0.0017453257076117112 0.4999969538288953
+v 0.7071067690849304 0.7088477868561263 0.4975286905846749
+v 1.2246468525851679e-16 1.0017392333654023 0.4965063024136719
+v 0 0.0017453257076117112 0.4999969538288953
+v 1.2246468525851679e-16 1.0017392333654023 0.4965063024136719
+v -0.7071067690849304 0.7088477868561263 0.4975286905846749
+v 0 0.0017453257076117112 0.4999969538288953
+v -0.7071067690849304 0.7088477868561263 0.4975286905846749
+v -1 0.001745325707611895 0.4999969538288953
+v 0 0.0017453257076117112 0.4999969538288953
+v -1 0.001745325707611895 0.4999969538288953
+v -0.7071067690849304 -0.705357135440903 0.5024652170731156
+v 0 0.0017453257076117112 0.4999969538288953
+v -0.7071067690849304 -0.705357135440903 0.5024652170731156
+v -2.4492937051703357e-16 -0.998248581950179 0.5034876052441187
+v 0 0.0017453257076117112 0.4999969538288953
+v 0.7071067690849304 -0.7088477868561263 -0.4975286905846749
+v 0 -1.0017392333654023 -0.4965063024136719
+v 0 -0.0017453257076117112 -0.4999969538288953
+v 1 -0.0017453257076117724 -0.4999969538288953
+v 0.7071067690849304 -0.7088477868561263 -0.4975286905846749
+v 0 -0.0017453257076117112 -0.4999969538288953
+v 0.7071067690849304 0.705357135440903 -0.5024652170731156
+v 1 -0.0017453257076117724 -0.4999969538288953
+v 0 -0.0017453257076117112 -0.4999969538288953
+v 1.2246468525851679e-16 0.998248581950179 -0.5034876052441187
+v 0.7071067690849304 0.705357135440903 -0.5024652170731156
+v 0 -0.0017453257076117112 -0.4999969538288953
+v -0.7071067690849304 0.705357135440903 -0.5024652170731156
+v 1.2246468525851679e-16 0.998248581950179 -0.5034876052441187
+v 0 -0.0017453257076117112 -0.4999969538288953
+v -1 -0.0017453257076115276 -0.4999969538288953
+v -0.7071067690849304 0.705357135440903 -0.5024652170731156
+v 0 -0.0017453257076117112 -0.4999969538288953
+v -0.7071067690849304 -0.7088477868561263 -0.4975286905846749
+v -1 -0.0017453257076115276 -0.4999969538288953
+v 0 -0.0017453257076117112 -0.4999969538288953
+v -2.4492937051703357e-16 -1.0017392333654023 -0.4965063024136719
+v -0.7071067690849304 -0.7088477868561263 -0.4975286905846749
+v 0 -0.0017453257076117112 -0.4999969538288953
+vt 0 1
+vt 0 0
+vt 0.125 1
+vt 0 0
+vt 0.125 0
+vt 0.125 1
+vt 0.125 1
+vt 0.125 0
+vt 0.25 1
+vt 0.125 0
+vt 0.25 0
+vt 0.25 1
+vt 0.25 1
+vt 0.25 0
+vt 0.375 1
+vt 0.25 0
+vt 0.375 0
+vt 0.375 1
+vt 0.375 1
+vt 0.375 0
+vt 0.5 1
+vt 0.375 0
+vt 0.5 0
+vt 0.5 1
+vt 0.5 1
+vt 0.5 0
+vt 0.625 1
+vt 0.5 0
+vt 0.625 0
+vt 0.625 1
+vt 0.625 1
+vt 0.625 0
+vt 0.75 1
+vt 0.625 0
+vt 0.75 0
+vt 0.75 1
+vt 0.75 1
+vt 0.75 0
+vt 0.875 1
+vt 0.75 0
+vt 0.875 0
+vt 0.875 1
+vt 0.875 1
+vt 0.875 0
+vt 1 1
+vt 0.875 0
+vt 1 0
+vt 1 1
+vt 1 0.5
+vt 0.8535534143447876 0.8535534143447876
+vt 0.5 0.5
+vt 0.8535534143447876 0.8535534143447876
+vt 0.5 1
+vt 0.5 0.5
+vt 0.5 1
+vt 0.1464466154575348 0.8535534143447876
+vt 0.5 0.5
+vt 0.1464466154575348 0.8535534143447876
+vt 0 0.5
+vt 0.5 0.5
+vt 0 0.5
+vt 0.1464466154575348 0.1464466154575348
+vt 0.5 0.5
+vt 0.1464466154575348 0.1464466154575348
+vt 0.5 0
+vt 0.5 0.5
+vt 0.5 0
+vt 0.8535534143447876 0.1464466154575348
+vt 0.5 0.5
+vt 0.8535534143447876 0.1464466154575348
+vt 1 0.5
+vt 0.5 0.5
+vt 0.8535534143447876 0.1464466154575348
+vt 1 0.5
+vt 0.5 0.5
+vt 0.5 0
+vt 0.8535534143447876 0.1464466154575348
+vt 0.5 0.5
+vt 0.1464466154575348 0.1464466154575348
+vt 0.5 0
+vt 0.5 0.5
+vt 0 0.5
+vt 0.1464466154575348 0.1464466154575348
+vt 0.5 0.5
+vt 0.1464466154575348 0.8535534143447876
+vt 0 0.5
+vt 0.5 0.5
+vt 0.5 1
+vt 0.1464466154575348 0.8535534143447876
+vt 0.5 0.5
+vt 0.8535534143447876 0.8535534143447876
+vt 0.5 1
+vt 0.5 0.5
+vt 1 0.5
+vt 0.8535534143447876 0.8535534143447876
+vt 0.5 0.5
+vn 0 -0.9999939076577902 0.0034906514152234207
+vn 0 -0.9999939076577902 0.0034906514152234207
+vn 0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn 0 -0.9999939076577902 0.0034906514152234207
+vn 0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn 0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn 0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn 0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn 1 -6.123196958087279e-17 2.1374076345626621e-19
+vn 0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn 1 -6.123196958087279e-17 2.1374076345626621e-19
+vn 1 -6.123196958087279e-17 2.1374076345626621e-19
+vn 1 -6.123196958087279e-17 2.1374076345626621e-19
+vn 1 -6.123196958087279e-17 2.1374076345626621e-19
+vn 0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn 1 -6.123196958087279e-17 2.1374076345626621e-19
+vn 0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn 0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn 0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn 0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn 1.2246468525851679e-16 0.9999939076577902 -0.0034906514152234207
+vn 0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn 1.2246468525851679e-16 0.9999939076577902 -0.0034906514152234207
+vn 1.2246468525851679e-16 0.9999939076577902 -0.0034906514152234207
+vn 1.2246468525851679e-16 0.9999939076577902 -0.0034906514152234207
+vn 1.2246468525851679e-16 0.9999939076577902 -0.0034906514152234207
+vn -0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn 1.2246468525851679e-16 0.9999939076577902 -0.0034906514152234207
+vn -0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn -0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn -0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn -0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn -1 1.836958955078092e-16 -6.412222441704118e-19
+vn -0.7071067690849304 0.7071024611485143 -0.002468263244220373
+vn -1 1.836958955078092e-16 -6.412222441704118e-19
+vn -1 1.836958955078092e-16 -6.412222441704118e-19
+vn -1 1.836958955078092e-16 -6.412222441704118e-19
+vn -1 1.836958955078092e-16 -6.412222441704118e-19
+vn -0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn -1 1.836958955078092e-16 -6.412222441704118e-19
+vn -0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn -0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn -0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn -0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn -2.4492937051703357e-16 -0.9999939076577902 0.0034906514152234207
+vn -0.7071067690849304 -0.7071024611485143 0.002468263244220373
+vn -2.4492937051703357e-16 -0.9999939076577902 0.0034906514152234207
+vn -2.4492937051703357e-16 -0.9999939076577902 0.0034906514152234207
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 0.0034906514152234207 0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+vn 0 -0.0034906514152234207 -0.9999939076577902
+f 1/1/1 2/2/2 3/3/3
+f 4/4/4 5/5/5 6/6/6
+f 7/7/7 8/8/8 9/9/9
+f 10/10/10 11/11/11 12/12/12
+f 13/13/13 14/14/14 15/15/15
+f 16/16/16 17/17/17 18/18/18
+f 19/19/19 20/20/20 21/21/21
+f 22/22/22 23/23/23 24/24/24
+f 25/25/25 26/26/26 27/27/27
+f 28/28/28 29/29/29 30/30/30
+f 31/31/31 32/32/32 33/33/33
+f 34/34/34 35/35/35 36/36/36
+f 37/37/37 38/38/38 39/39/39
+f 40/40/40 41/41/41 42/42/42
+f 43/43/43 44/44/44 45/45/45
+f 46/46/46 47/47/47 48/48/48
+f 49/49/49 50/50/50 51/51/51
+f 52/52/52 53/53/53 54/54/54
+f 55/55/55 56/56/56 57/57/57
+f 58/58/58 59/59/59 60/60/60
+f 61/61/61 62/62/62 63/63/63
+f 64/64/64 65/65/65 66/66/66
+f 67/67/67 68/68/68 69/69/69
+f 70/70/70 71/71/71 72/72/72
+f 73/73/73 74/74/74 75/75/75
+f 76/76/76 77/77/77 78/78/78
+f 79/79/79 80/80/80 81/81/81
+f 82/82/82 83/83/83 84/84/84
+f 85/85/85 86/86/86 87/87/87
+f 88/88/88 89/89/89 90/90/90
+f 91/91/91 92/92/92 93/93/93
+f 94/94/94 95/95/95 96/96/96

models/quadruped/urdf/quadruped.urdf

+<?xml version="1.0" encoding="utf-8"?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from quadroped.xacro                | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="quadroped" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
+  <link name="base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="2.0"/>
+      <inertia ixx="0.0270833333333" ixy="0" ixz="0" iyy="0.00708333333333" iyz="0" izz="0.0333333333333"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://quadruped/urdf/cube.obj" scale="0.2 0.4 0.05"/>
+        <!-- <box size="${base_w} ${base_l} ${base_h}"/> -->
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.2 0.4 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- <xacro:leg prefix="FL" rx="${0.}" ry="${0.}"/> -->
+  <joint name="FL_HFE" type="revolute">
+    <parent link="base_link"/>
+    <child link="FL_upperleg"/>
+    <limit effort="1000" lower="-10" upper="10" velocity="1000"/>
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="-0.1 0.2 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="FL_upperleg">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.08"/>
+      <geometry>
+        <!-- <cylinder length="${upper_leg_length}" radius="${upper_leg_radius}"/> -->
+        <mesh filename="package://quadruped/urdf/cylinder.obj" scale="0.025 0.025 0.16"/>
+      </geometry>
+      <material name="grey">
+        <color rgba=".4 .4 .4 .1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.08"/>
+      <geometry>
+        <cylinder length="0.16" radius="0.025"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.0793"/>
+      <mass value="0.162"/>
+      <inertia ixx="0.000469609" ixy="0.0" ixz="0.0" iyy="0.000469609" iyz="0.0" izz="1e-6"/>
+    </inertial>
+  </link>
+  <joint name="FL_KFE" type="revolute">
+    <parent link="FL_upperleg"/>
+    <child link="FL_shank"/>
+    <limit effort="1000" velocity="10000"/>
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="0 0 -0.16"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="FL_shank">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <restitution value="0.5"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.08"/>
+      <geometry>
+        <!-- <cylinder length="${upper_leg_length}" radius="${upper_leg_radius}"/> -->
+        <mesh filename="package://quadruped/urdf/cylinder.obj" scale="0.025 0.025 0.16"/>
+      </geometry>
+      <material name="grey">
+        <color rgba=".4 .4 .4 .1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.08"/>
+      <geometry>
+        <cylinder length="0.16" radius="0.025"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.0756"/>
+      <mass value="0.021"/>
+      <inertia ixx="0.000087575" ixy="0.0" ixz="0.0" iyy="0.000087575" iyz="0.0" izz="1e-6"/>
+    </inertial>
+  </link>
+  <joint name="FL_END" type="fixed">
+    <parent link="FL_shank"/>
+    <child link="FL_contact"/>
+    <origin rpy="0 0 0" xyz="0 0 -0.16"/>
+  </joint>
+  <link name="FL_contact">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <restitution value="0.5"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!-- <sphere radius="${upper_leg_radius}"/> -->
+        <mesh filename="package://quadruped/urdf/sphere.obj" scale="0.025 0.025 0.025"/>
+      </geometry>
+      <material name="red">
+        <color rgba="1. 0. 0. 1."/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.025"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0." ixy="0.0" ixz="0.0" iyy="0." iyz="0.0" izz="0."/>
+    </inertial>
+  </link>
+  <joint name="FR_HFE" type="revolute">
+    <parent link="base_link"/>
+    <child link="FR_upperleg"/>
+    <limit effort="1000" lower="-10" upper="10" velocity="1000"/>
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="0.1 0.2 0"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="FR_upperleg">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.08"/>
+      <geometry>
+        <!-- <cylinder length="${upper_leg_length}" radius="${upper_leg_radius}"/> -->
+        <mesh filename="package://quadruped/urdf/cylinder.obj" scale="0.025 0.025 0.16"/>
+      </geometry>
+      <material name="grey">
+        <color rgba=".4 .4 .4 .1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.08"/>
+      <geometry>
+        <cylinder length="0.16" radius="0.025"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.0793"/>
+      <mass value="0.162"/>
+      <inertia ixx="0.000469609" ixy="0.0" ixz="0.0" iyy="0.000469609" iyz="0.0" izz="1e-6"/>
+    </inertial>
+  </link>
+  <joint name="FR_KFE" type="revolute">
+    <parent link="FR_upperleg"/>
+    <child link="FR_shank"/>
+    <limit effort="1000" velocity="10000"/>
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="0 0 -0.16"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="FR_shank">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <restitution value="0.5"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.08"/>
+      <geometry>
+        <!-- <cylinder length="${upper_leg_length}" radius="${upper_leg_radius}"/> -->
+        <mesh filename="package://quadruped/urdf/cylinder.obj" scale="0.025 0.025 0.16"/>
+      </geometry>
+      <material name="grey">
+        <color rgba=".4 .4 .4 .1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.08"/>
+      <geometry>
+        <cylinder length="0.16" radius="0.025"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.0756"/>
+      <mass value="0.021"/>
+      <inertia ixx="0.000087575" ixy="0.0" ixz="0.0" iyy="0.000087575" iyz="0.0" izz="1e-6"/>
+    </inertial>
+  </link>
+  <joint name="FR_END" type="fixed">
+    <parent link="FR_shank"/>
+    <child link="FR_contact"/>
+    <origin rpy="0 0 0" xyz="0 0 -0.16"/>
+  </joint>
+  <link name="FR_contact">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <restitution value="0.5"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!-- <sphere radius="${upper_leg_radius}"/> -->
+        <mesh filename="package://quadruped/urdf/sphere.obj" scale="0.025 0.025 0.025"/>
+      </geometry>
+      <material name="red">
+        <color rgba="1. 0. 0. 1."/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.025"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0." ixy="0.0" ixz="0.0" iyy="0." iyz="0.0" izz="0."/>
+    </inertial>
+  </link>
+  <joint name="BL_HFE" type="revolute">