robotic 0.2.9.dev1__cp310-cp310-manylinux2014_x86_64.whl → 0.2.9.dev2__cp310-cp310-manylinux2014_x86_64.whl

robotic/rai-robotModels/ranger/ranger_clean.g

@@ -1,5 +1,5 @@
 base_link: { mass: 0.162716, inertia: [0.0007237, -1.55745e-08, -3.42379e-12, 0.000744047, 2.24935e-07, 2.14413e-05] },
-base_link_0(base_link): { Q: [0, 0, 0, 0.000796327, 1, 0, 0], shape: mesh, color: [0.792157, 0.819608, 0.933333, 1], mesh: <meshes/ranger_mini3.ply>, visual: True },
+base_link_0(base_link): { Q: [0, 0, 0, 1, -1, 0, 0], shape: mesh, color: [0.792157, 0.819608, 0.933333, 1], mesh: <meshes/ranger_mini3.h5>, visual: True },
 fr_joint_origin(base_link): { Q: [0.25, -0.19, -0.1] },
 fl_joint_origin(base_link): { Q: [0.25, 0.19, -0.1, -3.67321e-06, 1, 0, 0] },
 br_joint_origin(base_link): { Q: [-0.25, -0.19, -0.1] },
@@ -12,7 +12,7 @@ fr_Link(fr_joint): { mass: 2.2786, inertia: [0.0063625, -1.0581e-07, -4.0831e-07
 fl_Link(fl_joint): { mass: 2.2786, inertia: [0.0063695, -2.1067e-08, -0.00016867, 0.006363, -1.0301e-08, 0.010458] },
 br_Link(br_joint): { mass: 2.2786, inertia: [0.0063627, -2.1069e-08, -1.587e-05, 0.006363, 9.3201e-09, 0.010465] },
 bl_Link(bl_joint): { mass: 2.2786, inertia: [0.0063656, -8.1498e-08, -0.00011049, 0.006363, 6.6847e-08, 0.010462] },
-fr_Link_0(fr_Link): { Q: [-0.248, -0.113, 0.155, 0.000563312, 0.000562864, 0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel_right.ply>, visual: True },
-fl_Link_0(fl_Link): { Q: [-0.248, 0.1265, 0.191, 0.000563312, -0.000562864, -0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel.ply>, visual: True },
-br_Link_0(br_Link): { Q: [-0.241, -0.11, 0.155, 0.000563312, 0.000562864, 0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel_right.ply>, visual: True },
-bl_Link_0(bl_Link): { Q: [-0.241, 0.13, 0.2, 0.000563312, -0.000562864, -0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel.ply>, visual: True }
+fr_Link_0(fr_Link): { Q: [-0.248, -0.113, 0.155, 0.000563312, 0.000562864, 0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel_right.h5>, visual: True },
+fl_Link_0(fl_Link): { Q: [-0.248, 0.1265, 0.191, 0.000563312, -0.000562864, -0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel.h5>, visual: True },
+br_Link_0(br_Link): { Q: [-0.241, -0.11, 0.155, 0.000563312, 0.000562864, 0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel_right.h5>, visual: True },
+bl_Link_0(bl_Link): { Q: [-0.241, 0.13, 0.2, 0.000563312, -0.000562864, -0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel.h5>, visual: True }

robotic/rai-robotModels/ranger/ranger_mini_conv.g

@@ -0,0 +1,14 @@
+base_link: { mass: 25.7056, com: [0.0326128, 0.0718265, -0.00166507], inertia: [0.711027, 0.000383359, 0.0416814, 1.36554, 0.00276036, 1.84066] },
+base_link_0(base_link): { Q: [0, 0, 0, 0.707388, -0.706825, 0, 0], shape: mesh, color: [0.792157, 0.819608, 0.933333, 1], mesh: <meshes/ranger_mini3.h5> },
+fr_joint_origin(base_link): { Q: [0.25, -0.19, -0.1] },
+fl_joint_origin(base_link): { Q: [0.25, 0.19, -0.1, -3.67321e-06, 1, 0, 0] },
+br_joint_origin(base_link): { Q: [-0.25, -0.19, -0.1] },
+bl_joint_origin(base_link): { Q: [-0.25, 0.19, -0.1, -3.67321e-06, 1, 0, 0] },
+fr_joint(fr_joint_origin): { joint: hingeZ, joint_scale: -1, mass: 1.34089, com: [-0.49413, -0.335584, 0.280301], inertia: [0.00931857, -3.63082e-06, -4.19248e-06, 0.0101184, 0.000264633, 0.00405613] },
+fl_joint(fl_joint_origin): { joint: hingeZ, joint_scale: -1, mass: 1.33115, com: [-0.493868, 0.257102, 0.304992], inertia: [0.00930652, 2.87129e-06, -2.07991e-05, 0.0101544, -0.000255253, 0.0040282] },
+br_joint(br_joint_origin): { joint: hingeZ, joint_scale: -1, mass: 1.34089, com: [-0.48713, -0.332584, 0.280301], inertia: [0.00931857, -3.63082e-06, -4.19248e-06, 0.0101184, 0.000264633, 0.00405613] },
+bl_joint(bl_joint_origin): { joint: hingeZ, joint_scale: -1, mass: 1.33115, com: [-0.486868, 0.260602, 0.313992], inertia: [0.00930652, 2.87129e-06, -2.07991e-05, 0.0101544, -0.000255253, 0.0040282] },
+fr_Link_0(fr_joint): { Q: [-0.248, -0.113, 0.155, 0.000563312, -0.000562864, 0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel_right.h5> },
+fl_Link_0(fl_joint): { Q: [-0.248, 0.1265, 0.191, 0.000563312, 0.000562864, -0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel.h5> },
+br_Link_0(br_joint): { Q: [-0.241, -0.11, 0.155, 0.000563312, -0.000562864, 0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel_right.h5> },
+bl_Link_0(bl_joint): { Q: [-0.241, 0.13, 0.2, 0.000563312, 0.000562864, -0.706825, 0.707388], shape: mesh, color: [0.79216, 0.81961, 0.93333, 1], mesh: <meshes/ranger_mini_v3_wheel.h5> }

robotic/rai-robotModels/robotiq/robotiq.g

@@ -1,11 +1,11 @@
 robotiq_base: { X: [0, 0, 1] }
 
-Include: <robotiq_clean.g>
+Include: <robotiq_arg2f_85_model_conv.g>
 Edit robotiq_arg2f_base_link (robotiq_base): {}
 
 # add F/T sensor
 
-ftsensor (robotiq_base): { Q: [0, 0, -.035], shape: mesh, color: [0.2, 0.2, 0.2], mesh: <meshes/visual/robotiq_ft300.ply> }
+ftsensor (robotiq_base): { Q: [0, 0, -.035], shape: mesh, color: [0.2, 0.2, 0.2], mesh: <meshes/robotiq_ft300.ply> }
  
 
 # define a gripper, palm and fingers

robotic/rai-robotModels/robotiq/robotiq_arg2f_85_model_conv.g

@@ -0,0 +1,21 @@
+robotiq_arg2f_base_link: { shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/robotiq_arg2f_85_base_link.h5>, mass: 0.16309, inertia: [0.00022626, 6.22636e-10, 6.5573e-11, 0.000188087, -3.11813e-07, 0.000141097] },
+finger_joint_origin(robotiq_arg2f_base_link): { Q: [0, -0.0306011, 0.054904, 6.12323e-17, 0, 0, 1] },
+left_inner_knuckle_joint_origin(robotiq_arg2f_base_link): { Q: [0, -0.0127, 0.06142, 6.12323e-17, 0, 0, 1] },
+right_outer_knuckle_joint_origin(robotiq_arg2f_base_link): { Q: [0, 0.0306011, 0.054904] },
+right_inner_knuckle_joint_origin(robotiq_arg2f_base_link): { Q: [0, 0.0127, 0.06142] },
+finger_joint(finger_joint_origin): { joint: hingeX, limits: [0, 0.8], shape: mesh, color: [0.792157, 0.819608, 0.933333, 1], mesh: <meshes/robotiq_arg2f_85_outer_knuckle.h5>, mass: 0.0141429, inertia: [3.26716e-06, -5.23289e-17, 8.54928e-17, 3.03476e-06, 1.77988e-07, 8.59178e-07] },
+left_inner_knuckle_joint(left_inner_knuckle_joint_origin): { joint: hingeX, limits: [0, 0.8757], mimic: finger_joint, shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/robotiq_arg2f_85_inner_knuckle.h5>, mass: 0.0293528, inertia: [1.18448e-05, -2.51482e-13, 9.42698e-14, 1.16071e-05, -5.32477e-06, 9.92039e-06] },
+right_outer_knuckle_joint(right_outer_knuckle_joint_origin): { joint: hingeX, limits: [0, 0.81], mimic: finger_joint, shape: mesh, color: [0.792157, 0.819608, 0.933333, 1], mesh: <meshes/robotiq_arg2f_85_outer_knuckle.h5>, mass: 0.0141429, inertia: [3.26716e-06, -5.23289e-17, 8.54928e-17, 3.03476e-06, 1.77988e-07, 8.59178e-07] },
+right_inner_knuckle_joint(right_inner_knuckle_joint_origin): { joint: hingeX, limits: [0, 0.8757], mimic: finger_joint, shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/robotiq_arg2f_85_inner_knuckle.h5>, mass: 0.0293528, inertia: [1.18448e-05, -2.51482e-13, 9.42698e-14, 1.16071e-05, -5.32477e-06, 9.92039e-06] },
+left_outer_finger_joint_origin(finger_joint): { Q: [0, 0.0315, -0.0041] },
+right_outer_finger_joint_origin(right_outer_knuckle_joint): { Q: [0, 0.0315, -0.0041] },
+left_outer_finger_joint(left_outer_finger_joint_origin): { joint: rigid, shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/robotiq_arg2f_85_outer_finger.h5>, mass: 0.0246725, inertia: [8.05877e-06, 5.27023e-16, -3.05353e-16, 9.48665e-06, -9.13935e-07, 2.56178e-06] },
+right_outer_finger_joint(right_outer_finger_joint_origin): { joint: rigid, shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/robotiq_arg2f_85_outer_finger.h5>, mass: 0.0246725, inertia: [8.05877e-06, 5.27023e-16, -3.05353e-16, 9.48665e-06, -9.13935e-07, 2.56178e-06] },
+left_inner_finger_joint_origin(left_outer_finger_joint): { Q: [0, 0.0061, 0.0471] },
+right_inner_finger_joint_origin(right_outer_finger_joint): { Q: [0, 0.0061, 0.0471] },
+left_inner_finger_joint(left_inner_finger_joint_origin): { joint: hingeX, limits: [0, 0.8757], mimic: finger_joint, shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/robotiq_arg2f_85_inner_finger.h5>, mass: 0.0205822, inertia: [5.68083e-06, -1.41221e-12, 4.89169e-12, 4.88007e-06, 1.15703e-06, 1.93071e-06] },
+right_inner_finger_joint(right_inner_finger_joint_origin): { joint: hingeX, limits: [0, 0.8757], mimic: finger_joint, shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/robotiq_arg2f_85_inner_finger.h5>, mass: 0.0205822, inertia: [5.68083e-06, -1.41221e-12, 4.89169e-12, 4.88007e-06, 1.15703e-06, 1.93071e-06] },
+left_inner_finger_pad_joint_origin(left_inner_finger_joint): { Q: [0, -0.0220203, 0.03242] },
+right_inner_finger_pad_joint_origin(right_inner_finger_joint): { Q: [0, -0.0220203, 0.03242] },
+left_inner_finger_pad_joint(left_inner_finger_pad_joint_origin): { joint: rigid, shape: box, size: [0.022, 0.00635, 0.0375], color: [0.9, 0.9, 0.9, 1], mass: 0.0120282, inertia: [1.73997e-05, 2.27364e-05, 6.30668e-06] },
+right_inner_finger_pad_joint(right_inner_finger_pad_joint_origin): { joint: rigid, shape: box, size: [0.022, 0.00635, 0.0375], color: [0.9, 0.9, 0.9, 1], mass: 0.0120282, inertia: [1.73997e-05, 2.27364e-05, 6.30668e-06] }

robotic/rai-robotModels/robotiq/robotiq_clean.g

@@ -1,24 +1,24 @@
 robotiq_arg2f_base_link: { mass: 0.22652, inertia: [0.00020005, -4.2442e-10, -2.9069e-10, 0.00017832, -3.4402e-08, 0.00013478] }
 robotiq_arg2f_base_link_0(robotiq_arg2f_base_link): { shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_base_link.ply>, meshscale: 0.001, visual: True }
-finger_joint_origin(robotiq_arg2f_base_link): { rel: [0, -0.0306011, 0.054904, 6.12323e-17, 0, 0, 1] }
-left_inner_knuckle_joint_origin(robotiq_arg2f_base_link): { rel: [0, -0.0127, 0.06142, 6.12323e-17, 0, 0, 1] }
-right_outer_knuckle_joint_origin(robotiq_arg2f_base_link): { rel: [0, 0.0306011, 0.054904, 1, 0, 0, 0] }
-right_inner_knuckle_joint_origin(robotiq_arg2f_base_link): { rel: [0, 0.0127, 0.06142, 1, 0, 0, 0] }
-finger_joint(finger_joint_origin): { joint: hingeX, limits: [0, 0.8, 2, -1, 1000], ctrl_limits: [2, -1, 1000] }
-left_inner_knuckle_joint(left_inner_knuckle_joint_origin): { joint: hingeX, limits: [0, 0.8757, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
-right_outer_knuckle_joint(right_outer_knuckle_joint_origin): { joint: hingeX, limits: [0, 0.81, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
-right_inner_knuckle_joint(right_inner_knuckle_joint_origin): { joint: hingeX, limits: [0, 0.8757, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
+robotiq_arg2f_base_link>finger_joint(robotiq_arg2f_base_link): { Q: [0, -0.0306011, 0.054904, 6.12323e-17, 0, 0, 1] }
+robotiq_arg2f_base_link>left_inner_knuckle_joint(robotiq_arg2f_base_link): { Q: [0, -0.0127, 0.06142, 6.12323e-17, 0, 0, 1] }
+robotiq_arg2f_base_link>right_outer_knuckle_joint(robotiq_arg2f_base_link): { Q: [0, 0.0306011, 0.054904, 1, 0, 0, 0] }
+robotiq_arg2f_base_link>right_inner_knuckle_joint(robotiq_arg2f_base_link): { Q: [0, 0.0127, 0.06142, 1, 0, 0, 0] }
+finger_joint(robotiq_arg2f_base_link>finger_joint): { joint: hingeX, limits: [0, 0.8, 2, -1, 1000], ctrl_limits: [2, -1, 1000] }
+left_inner_knuckle_joint(robotiq_arg2f_base_link>left_inner_knuckle_joint): { joint: hingeX, limits: [0, 0.8757, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
+right_outer_knuckle_joint(robotiq_arg2f_base_link>right_outer_knuckle_joint): { joint: hingeX, limits: [0, 0.81, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
+right_inner_knuckle_joint(robotiq_arg2f_base_link>right_inner_knuckle_joint): { joint: hingeX, limits: [0, 0.8757, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
 left_outer_knuckle_0(finger_joint): { shape: mesh, color: [0.792157, 0.819608, 0.933333, 1], mesh: <meshes/visual/robotiq_arg2f_85_outer_knuckle.ply>, meshscale: 0.001, visual: True }
-left_outer_finger_0(finger_joint): { rel: [0, 0.0315, -0.0041, 1, 0, 0, 0], shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_outer_finger.ply>, meshscale: 0.001, visual: True }
-left_inner_finger_joint_origin(finger_joint): { rel: [0, 0.0376, 0.043, 1, 0, 0, 0] }
+left_outer_finger_0(finger_joint): { Q: [0, 0.0315, -0.0041, 1, 0, 0, 0], shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_outer_finger.ply>, meshscale: 0.001, visual: True }
+left_outer_finger>left_inner_finger_joint(finger_joint): { Q: [0, 0.0376, 0.043, 1, 0, 0, 0] }
 left_inner_knuckle_0(left_inner_knuckle_joint): { shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_inner_knuckle.ply>, meshscale: 0.001, visual: True }
 right_outer_knuckle_0(right_outer_knuckle_joint): { shape: mesh, color: [0.792157, 0.819608, 0.933333, 1], mesh: <meshes/visual/robotiq_arg2f_85_outer_knuckle.ply>, meshscale: 0.001, visual: True }
-right_outer_finger_0(right_outer_knuckle_joint): { rel: [0, 0.0315, -0.0041, 1, 0, 0, 0], shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_outer_finger.ply>, meshscale: 0.001, visual: True }
-right_inner_finger_joint_origin(right_outer_knuckle_joint): { rel: [0, 0.0376, 0.043, 1, 0, 0, 0] }
+right_outer_finger_0(right_outer_knuckle_joint): { Q: [0, 0.0315, -0.0041, 1, 0, 0, 0], shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_outer_finger.ply>, meshscale: 0.001, visual: True }
+right_outer_finger>right_inner_finger_joint(right_outer_knuckle_joint): { Q: [0, 0.0376, 0.043, 1, 0, 0, 0] }
 right_inner_knuckle_0(right_inner_knuckle_joint): { shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_inner_knuckle.ply>, meshscale: 0.001, visual: True }
-left_inner_finger_joint(left_inner_finger_joint_origin): { joint: hingeX, limits: [0, 0.8757, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
-right_inner_finger_joint(right_inner_finger_joint_origin): { joint: hingeX, limits: [0, 0.8757, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
+left_inner_finger_joint(left_outer_finger>left_inner_finger_joint): { joint: hingeX, limits: [0, 0.8757, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
+right_inner_finger_joint(right_outer_finger>right_inner_finger_joint): { joint: hingeX, limits: [0, 0.8757, 2, -1, 1000], mimic: "finger_joint", ctrl_limits: [2, -1, 1000] }
 left_inner_finger_0(left_inner_finger_joint): { shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_inner_finger.ply>, meshscale: 0.001, visual: True }
-left_inner_finger_pad_0(left_inner_finger_joint): { rel: [0, -0.0220203, 0.03242, 1, 0, 0, 0], shape: box, size: [0.022, 0.00635, 0.0375, 0], color: [0.9, 0.9, 0.9, 1], visual: True }
+left_inner_finger_pad_0(left_inner_finger_joint): { Q: [0, -0.0220203, 0.03242, 1, 0, 0, 0], shape: box, size: [0.022, 0.00635, 0.0375, 0], color: [0.9, 0.9, 0.9, 1], visual: True }
 right_inner_finger_0(right_inner_finger_joint): { shape: mesh, color: [0.1, 0.1, 0.1, 1], mesh: <meshes/visual/robotiq_arg2f_85_inner_finger.ply>, meshscale: 0.001, visual: True }
-right_inner_finger_pad_0(right_inner_finger_joint): { rel: [0, -0.0220203, 0.03242, 1, 0, 0, 0], shape: box, size: [0.022, 0.00635, 0.0375, 0], color: [0.9, 0.9, 0.9, 1], visual: True }
+right_inner_finger_pad_0(right_inner_finger_joint): { Q: [0, -0.0220203, 0.03242, 1, 0, 0, 0], shape: box, size: [0.022, 0.00635, 0.0375, 0], color: [0.9, 0.9, 0.9, 1], visual: True }

robotic/rai-robotModels/scenarios/ballFinger.g

@@ -1,5 +1,5 @@
 base { X:[0 0 .5], multibody, multibody_gravity: false }
-jointX(base){ joint:transX, mass:.01, limits: [-1.,1.], motorLambda: .01, motorMass: .01, sampleUniform: 1. }
-jointY(jointX){ joint:transY, mass:.01, limits: [-1.,1.], motorLambda: .01, motorMass: .01, sampleUniform: 1. }
+jointX(base){ joint:transX, mass:.01, inertia:[.01,.01,.01], limits: [-1.,1.], motorLambda: .01, motorMass: .01, sampleUniform: 1. }
+jointY(jointX){ joint:transY, mass:.01, inertia:[.01,.01,.01], limits: [-1.,1.], motorLambda: .01, motorMass: .01, sampleUniform: 1. }
 jointZ(jointY){ joint:transZ, limits: [-1.,1.], motorLambda: .01, motorMass: .01, sampleUniform: 1. }
 finger(jointZ){ shape:sphere, size:[.02], color:[.5 1 1], contact:1, mass: .1 }

robotic/rai-robotModels/tests/compound.g

@@ -1,11 +1,8 @@
 
-world: {}
-
 table: { X: [0, 0, .1], shape: ssBox, size: [2, 2, .1, .02], color: [.3, .3, .3] }
 
 obj: { X: [0, 0, 2], shape: marker, size: [.3] }
-
-part1 (obj): { Q: "t(0 -.5 0) d(90 0 1 0)", shape: capsule, size: [.5, .05], color: [.9, .9, .9], mass: 1 }
-part2 (obj): { Q: "t(0 .5 0)", shape: capsule, size: [1., .05], color: [.9, .9, .9] }
-part3 (obj): { Q: "t(0 .25 0) d(90 1 0 0)", shape: capsule, size: [1.5, .05], color: [.9, .9, .9] }
+part1 (obj): { Q: "t(0 -.5 0) d(90 0 1 0)", shape: sphere, size: [.15], color: [.9, .9, .9], mass: 1 }
+part2 (obj): { Q: "t(0 .5 0)", shape: capsule, size: [1., .05], color: [.9, .9, .9], mass: 1 }
+part3 (obj): { Q: "t(0 .25 0) d(90 1 0 0)", shape: capsule, size: [1.5, .05], color: [.9, .9, .9], mass: 1 }
 part4 (obj): { Q: "t(.3 1. 0) d(90 1 0 0)", shape: ssBox, size: [.5, .5, .5, .05], color: [.9, .9, .9], mass: 1 }

robotic/rai-robotModels/ur10/ur10.g

@@ -1,5 +1,5 @@
 Prefix: "ur_"
-Include: <ur10_clean.g>
+Include: <ur10_conv.g>
 Prefix: false
 
 Edit ur_world_joint_origin: { rel: [0, 0, 0] }

robotic/rai-robotModels/ur10/ur10_conv.g

@@ -0,0 +1,17 @@
+world: {  },
+world_joint_origin(world): { Q: [0, 0, 1] },
+world_joint(world_joint_origin): { joint: rigid, shape: mesh, color: [0.371451, 0.371451, 0.371451, 1], mesh: <meshes/base.h5>, mass: 0.328598, inertia: [0.000876246, -3.50721e-07, -2.99737e-08, 0.000730367, 2.05527e-06, 0.00143972] },
+shoulder_pan_joint_origin(world_joint): { Q: [0, 0, 0.1273] },
+shoulder_pan_joint(shoulder_pan_joint_origin): { joint: hingeZ, limits: [-6.28319, 6.28319], shape: mesh, mesh: <meshes/shoulder.h5>, mass: 0.909582, inertia: [0.00565017, -6.93355e-06, -8.74462e-06, 0.00537262, -4.97716e-05, 0.00439801] },
+shoulder_lift_joint_origin(shoulder_pan_joint): { Q: [0, 0.220941, 0, 0.707107, 0, 0.707107, 0] },
+shoulder_lift_joint(shoulder_lift_joint_origin): { joint: hingeY, limits: [-6.28319, 6.28319], shape: mesh, mesh: <meshes/upperarm.h5>, mass: 2.43496, inertia: [0.159184, 1.47112e-05, -8.39265e-05, 0.157564, 0.000647215, 0.0100854] },
+elbow_joint_origin(shoulder_lift_joint): { Q: [0, -0.1719, 0.612] },
+elbow_joint(elbow_joint_origin): { joint: hingeY, limits: [-6.28319, 6.28319], shape: mesh, mesh: <meshes/forearm.h5>, mass: 1.75122, inertia: [0.0968559, -9.76006e-06, 1.57892e-05, 0.0958587, 0.000947383, 0.00463623] },
+wrist_1_joint_origin(elbow_joint): { Q: [0, 0, 0.5723, 0.707107, 0, 0.707107, 0] },
+wrist_1_joint(wrist_1_joint_origin): { joint: hingeY, limits: [-6.28319, 6.28319], shape: mesh, mesh: <meshes/wrist1.h5>, mass: 0.361159, inertia: [0.000895857, 1.09527e-06, 9.37493e-07, 0.000848918, -1.01451e-05, 0.000646337] },
+wrist_2_joint_origin(wrist_1_joint): { Q: [0, 0.1149, 0] },
+wrist_2_joint(wrist_2_joint_origin): { joint: hingeZ, limits: [-6.28319, 6.28319], shape: mesh, mesh: <meshes/wrist2.h5>, mass: 0.361992, inertia: [0.000898399, -5.86223e-07, -1.0802e-06, 0.000648368, -1.06317e-05, 0.000850769] },
+wrist_3_joint_origin(wrist_2_joint): { Q: [0, 0, 0.1157] },
+wrist_3_joint(wrist_3_joint_origin): { joint: hingeY, limits: [-6.28319, 6.28319], shape: mesh, color: [0.464314, 0.464314, 0.464314, 1], mesh: <meshes/wrist3.h5>, mass: 0.108345, inertia: [8.36376e-05, 4.10321e-09, 1.31385e-08, 0.000134503, -9.21218e-09, 8.00354e-05] },
+ee_fixed_joint_origin(wrist_3_joint): { Q: [0, 0.0922, 0, 0.707107, 0, 0, 0.707107] },
+ee_fixed_joint(ee_fixed_joint_origin): { joint: rigid }

robotic/ry-urdfConvert.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python3
+
+# from config_mujoco import *
+# from config_urdf import *
+# from mesh_helper import *
+from robotic.src.yaml_helper import *
+import robotic as ry
+import argparse
+import glob
+import os
+
+parser = argparse.ArgumentParser(
+    description='Utility to clean meshes in meshes/')
+
+parser.add_argument('file', type=str, help='urdf file', nargs='?', default='none')
+
+parser.add_argument('-view', help='view mesh', action="store_true", default=True)
+parser.add_argument('-flipDaeYZ', help='view mesh', action="store_true", default=False)
+parser.add_argument('-recomputeInertias', help='view mesh', action="store_true")
+parser.add_argument('-processMeshes', help='view mesh', action="store_true", default=True)
+parser.add_argument('-meshlab', help='apply meshlab filters', action="store_true", default=False)
+
+def main():
+    args = parser.parse_args()
+
+    if args.file=='none':
+        # args.file = '/home/mtoussai/git/rai-robotModels/pr2/pr2.urdf'
+        args.file = '/home/mtoussai/git/rai-robotModels/panda/panda_arm_hand.urdf'
+        # args.file = '/home/mtoussai/git/rai-robotModels/g1/g1_description/g1_29dof.urdf'
+        # args.file = '/home/mtoussai/git/rai-robotModels/ranger/ranger_mini.urdf'
+
+    print('=== URDF CONVERT ===', args.file)
+
+    path, file = os.path.split(args.file)
+    filebase, _ = os.path.splitext(file)
+
+    if args.flipDaeYZ:
+        ry.params_add({'assimp/daeFlipYZ': False})
+
+    C = ry.URDFLoader(args.file, visualsOnly=True, meshPathRemove='package://').C
+
+    C.processStructure(False, True, False, False)
+    C.processInertias(args.recomputeInertias)
+    C.processStructure(False, True, False, False)
+
+    os.system('rm -Rf meshes/')
+    C.writeMeshes('meshes/', copyTextures=True)
+
+    print('#frames: ', C.getFrameDimension())
+    with open(f'{filebase}_conv.g', 'w') as fil:
+        #yaml.dump(C.asDict(), file, default_flow_style=False)
+        fil.write(C.write())
+
+    yaml_write_dict(C.asDict(), f'{filebase}_conv.yml')
+
+    C.view(True)
+    # C.animate()
+
+    if args.processMeshes:
+        for file in sorted(glob.glob('meshes/*.h5')):
+
+            M = ry.MeshHelper(file)
+            if M.mesh is None:
+                continue
+
+            M.repair(mergeTolerance=1e-4)
+            print('  watertight:', M.mesh.is_watertight)
+            print('  oriented:', M.mesh.is_winding_consistent)
+            M.report()
+            M.export_h5()
+
+if __name__ == "__main__":
+    main()

robotic/src/config_mujoco.py

@@ -0,0 +1,237 @@
+
+import os
+import numpy as np
+import robotic as ry
+import xml.etree.ElementTree as ET
+from copy import copy
+
+class MujocoLoader():
+
+    def __init__(self, file, visualsOnly=True):
+        self.visualsOnly = visualsOnly
+        self.debug_counter = 0
+        self.muj2rai_joint_map = {
+            '1 0 0': ry.JT.hingeX,
+            '0 1 0': ry.JT.hingeY,
+            '0 0 1': ry.JT.hingeZ,
+            '-1 0 0': ry.JT.hingeX,
+            '0 -1 0': ry.JT.hingeY,
+            '0 0 -1': ry.JT.hingeZ,
+        }
+
+        tree = ET.parse(file)
+        path, _ = os.path.split(file)
+        root = tree.getroot()
+
+        self.materials = {}    
+        self.textures = {}
+        self.meshes = {}
+        self.load_assets(root, path)
+        self.bodyCount = -1
+
+        self.C = ry.Config()
+        self.base = self.C.addFrame('base')
+        self.base.addAttributes({'multibody':True})
+        self.base.setQuaternion([0,0,0,1])
+        self.add_node(root, self.base, path, 0)
+
+    def as_floats(self, input_string):
+        return [float(num) for num in input_string.replace(',', ' ').split()]
+    
+    def load_assets(self, root, path):
+        texs = root.findall('.//texture')
+        for tex in texs:
+            name = tex.attrib.get('name', '')
+            file = tex.attrib.get('file', '')
+            self.textures[name] = os.path.join(path, file)
+
+        maters = root.findall('.//material')
+        for mater in maters:
+            name = mater.attrib.get('name', '')
+            color = mater.attrib.get('rgba', '')
+            texture_name = mater.attrib.get('texture', '')
+            if texture_name:
+                self.materials[name] = self.textures[texture_name]
+            elif color:
+                self.materials[name] = color
+            else:
+                self.materials[name] = ''
+
+        for mesh in root.findall('.//mesh'):
+            name = mesh.attrib.get('name', '')
+            file = mesh.attrib.get('file', '')
+            if file.startswith('visual') or file.startswith('collision'): #HACK: the true path is hidden in some compiler attribute
+                file = 'meshes/'+file
+            mesh.attrib['file'] = file
+            self.meshes[name] = mesh.attrib
+    
+    def add_node(self, node, f_parent, path, level):
+        if 'file' in node.attrib:
+            file = node.attrib['file']
+            node.attrib['file'] = os.path.join(path, file)
+
+        print('|'+level*'  ', node.tag, node.attrib)
+
+        if node.tag == 'include':
+            file = node.attrib['file'] 
+            tree = ET.parse(file)
+            root = tree.getroot()
+            path, _ = os.path.split(file)
+            self.load_assets(root, path)
+            self.add_node(root, f_parent, path, level+1)
+
+        f_body = None
+
+        if node.tag == 'body':
+            f_body = self.add_body(node, f_parent)
+
+        for child in node:
+            if f_body is None:
+                self.add_node(child, f_parent, path, level+1)
+            else:
+                self.add_node(child, f_body, path, level+1)
+
+    def add_body(self, body, f_parent):
+        self.bodyCount += 1
+        body_name = body.attrib.get('name', f'body_{self.bodyCount}')
+
+        # if self.C.getFrame(body_name, False):
+        body_name = f'{body_name}_{self.bodyCount}'
+        f_body = self.C.addFrame(body_name)
+        f_body.setParent(f_parent)
+        self.setRelativePose(f_body, body.attrib)
+
+        for i, joint in enumerate(body.findall('./joint')):
+            axis = joint.attrib.get('axis', None)
+            limits = joint.attrib.get('range', None)
+            joint_name = joint.attrib.get('name', f'{body_name}_joint{i*'_'}')
+            f_origin = self.C.addFrame(f'{joint_name}_origin')
+            f_origin.setParent(f_body)
+            self.setRelativePose(f_origin, joint.attrib)
+            f_origin.unLink()
+            f_origin.setParent(f_parent, True)
+
+            if axis:
+                if axis in self.muj2rai_joint_map:
+                    axis = self.muj2rai_joint_map[axis]
+                else:
+                    vec1 = np.array([0., 0., 1.])
+                    vec2 = np.array(self.as_floats(axis))
+                    quat = ry.Quaternion().setDiff(vec1, vec2).getArr()
+                    f_origin.setRelativeQuaternion(quat)
+                    axis = ry.JT.hingeZ
+            else:
+                axis = ry.JT.hingeZ
+
+            if joint.attrib.get('type', 'hinge')=='slide':
+                trans_map = {
+                    ry.JT.hingeX: ry.JT.transX,
+                    ry.JT.hingeY: ry.JT.transY,
+                    ry.JT.hingeZ: ry.JT.transZ}
+                axis = trans_map[axis]
+
+            if not limits:
+                limits = '-1 1'
+
+            # if self.C.getFrame(joint_name, False):
+            #     joint_name = f'{joint_name}_{self.bodyCount}'
+            f_joint = self.C.addFrame(joint_name)
+            f_joint.setParent(f_origin)
+            f_joint.setJoint(axis, self.as_floats(limits))
+            
+            # relink body:
+            f_parent = f_joint
+            f_body.unLink()
+            f_body.setParent(f_parent, True)
+
+        for i, geom in enumerate(body.findall('./geom')):
+            isColl = ('contype' in geom.attrib and geom.attrib.get('contype', '')!='0') or 'col' in geom.attrib.get('class','')
+            if self.visualsOnly and isColl:
+                continue
+
+            f_shape = self.C.addFrame(f'{body_name}_shape{i}')
+            f_shape.setParent(f_body)
+
+            if 'mesh' in geom.attrib:
+                mesh = geom.attrib.get('mesh', '')
+                material_name = geom.attrib.get('material', '')
+                texture_path = self.materials.get(material_name, None)
+                meshfile = self.meshes[mesh]['file']
+                scale = self.as_floats(self.meshes[mesh].get('scale', '1 1 1'))
+
+                f_shape.setMeshFile(meshfile, scale[0])
+                
+                if texture_path:
+                    if len(texture_path.split()) == 4:  # Is a color rgba
+                        f_shape.setColor(self.as_floats(texture_path))
+
+            elif 'type' in geom.attrib:
+                size = self.as_floats(geom.attrib['size'])
+                if geom.attrib['type']=='capsule':
+                    if 'fromto' in geom.attrib:
+                        fromto = self.as_floats(geom.attrib['fromto'])
+                        a, b = np.array(fromto[:3]), np.array(fromto[3:])
+                        l = np.linalg.norm(b-a)
+                        q = ry.Quaternion().setDiff([0,0,1],(b-a)/l)
+                        f_shape.setRelativePosition(0.5*(a+b))
+                        f_shape.setRelativeQuaternion(q.getArr())
+                        f_shape.setShape(ry.ST.capsule, [l, size[0]])
+                    elif len(size)==2:
+                        f_shape.setShape(ry.ST.capsule, [2.*size[1], size[0]])
+
+                elif geom.attrib['type']=='cylinder':
+                    if len(size)==2:
+                        f_shape.setShape(ry.ST.cylinder, [2.*size[1], size[0]])
+
+                elif geom.attrib['type']=='box':
+                    assert len(size)==3
+                    f_shape.setShape(ry.ST.box, [2.*f for f in size])
+                    if geom.attrib.get('material', None):
+                        texture_path = self.materials[geom.attrib.get('material', None)]
+                        if len(texture_path.split()) == 4:  # Is a color rgba
+                            f_shape.setColor(self.as_floats(texture_path))
+                        else:
+                            print('applying to box:', texture_path)
+                            #TODO incorperate <texrepeat> tag correctly    
+                            uv_coords = np.array([
+                            [0, 0],  # vertex 0
+                            [1, 0],  # vertex 1
+                            [1, 1],  # vertex 2
+                            [0, 1],  # vertex 3
+                            [0, 0],  # vertex 4
+                            [1, 0],  # vertex 5
+                            [1, 1],  # vertex 6
+                            [0, 1]   # vertex 7
+                            ])
+                            
+                            f_shape.setTextureFile(texture_path, uv_coords)
+
+                elif geom.attrib['type']=='sphere':
+                    if len(size)==1:
+                        f_shape.setShape(ry.ST.sphere, size)
+                
+            self.setRelativePose(f_shape, geom.attrib)
+
+            if geom.attrib.get('rgba', None):
+                if geom.attrib.get('material', None) is None:
+                    f_shape.setColor(self.as_floats(geom.attrib['rgba']))
+
+            elif isColl:
+                f_shape.setColor([1,0,0,.2])
+                
+        return f_body
+
+    def setRelativePose(self, f, attrib):
+        pos = attrib.get('pos', None)
+        if pos:
+            f.setRelativePosition(self.as_floats(pos))
+        
+        quat = attrib.get('quat', None)
+        if quat:
+            f.setRelativeQuaternion(self.as_floats(quat))
+        
+        rpy = attrib.get('euler', None)
+        if rpy:
+            q = ry.Quaternion()
+            q.setRollPitchYaw(self.as_floats(rpy))
+            f.setRelativeQuaternion(q.getArr())