Fix panda demo

multiverse/modules/multiverse_connectors/src/mujoco/scripts/mujoco_compile.py

@@ -271,6 +271,7 @@ def add_key_frame_element(root: ET.Element, keyframe_dict: Dict[str, ET.Element]
             key_element.attrib.pop("ctrl")
         if key_element.get("qpos") != "" or key_element.get("ctrl") != "":
             keyframe_element.append(key_element)
+    return None
 
 
 def get_qpos_and_ctrl(m: mujoco.MjModel, joint_state: Dict[str, float]) -> (List[float], List[float]):
@@ -346,20 +347,19 @@ def indent(elem, space="    ", level=0):
 
 def add_mocap(save_xml_path: str,
               tree: ET.ElementTree,
-              mocap_dict: Dict[str, List[float]],
+              mocap_dict: Dict[str, Tuple[str, List[float], List[float]]],
               m: mujoco.MjModel,
               d: mujoco.MjData) -> None:
     worldbody_element = ET.Element("worldbody")
     root = tree.getroot()
     root.append(worldbody_element)
-    for mocap_name, body_id in mocap_dict.items():
-        mocap_pos = d.xpos[body_id]
-        mocap_quat = d.xquat[body_id]
+    for mocap_name, (body_id, body_pos, body_quat) in mocap_dict.items():
         body_element = ET.Element(
             "body",
-            {"name": mocap_name, "mocap": "true",
-             "pos": " ".join(map(str, mocap_pos)),
-             "quat": " ".join(map(str, mocap_quat))},
+            {"name": mocap_name, 
+             "mocap": "true",
+             "pos": " ".join(map(str, body_pos)),
+             "quat": " ".join(map(str, body_quat))},
         )
         geom_adr = m.body_geomadr[body_id]
         geom_num = m.body_geomnum[body_id]
@@ -459,20 +459,17 @@ def apply_references(save_xml_path: str, references: Dict[str, Dict[str, any]])
             raise ValueError(f"body1 or body2 must be valid body names in the model")
 
         mocap_name = None
-        body_name = None
         body_id = None
         if body1_id == -1:
             mocap_name = body1_name
-            body_name = body2_name
             body_id = body2_id
         if body2_id == -1:
             mocap_name = body2_name
-            body_name = body1_name
             body_id = body1_id
         if mocap_name is not None and body_id is not None:
             body_pos = d.xpos[body_id]
-            print(f"Add {mocap_name} as mocap of {body_name} at {body_pos} to the model")
-            mocap_dict[mocap_name] = body_id
+            body_quat = d.xquat[body_id]
+            mocap_dict[mocap_name] = (body_id, body_pos, body_quat)
 
         weld_element = ET.Element("weld", {"name": reference_name})
         for prop_key, prop_value in reference_props.items():
@@ -483,8 +480,29 @@ def apply_references(save_xml_path: str, references: Dict[str, Dict[str, any]])
         equality_element.append(weld_element)
 
     add_mocap(save_xml_path, tree, mocap_dict, m, d)
+
+    mujoco.mj_resetDataKeyframe(m, d, 0)
+    mujoco.mj_step(m, d)
+    mpos_list_str = "0 0 0"
+    mquat_list_str = "1 0 0 0"
+    for mocap_name, (body_id, _, _) in mocap_dict.items():
+        body_pos = d.xpos[body_id]
+        body_quat = d.xquat[body_id]
+        mpos_list_str += f" {body_pos[0]} {body_pos[1]} {body_pos[2]}"
+        mquat_list_str += f" {body_quat[0]} {body_quat[1]} {body_quat[2]} {body_quat[3]}"
+
+    keyframe_element = root.find(".//keyframe")
+    root.remove(keyframe_element)
+    key_element = keyframe_element.find(".//key")
+    keyframe_element.remove(key_element)
+    key_element.set("mpos", mpos_list_str)
+    key_element.set("mquat", mquat_list_str)
+    keyframe_element.append(key_element)
+    root.append(keyframe_element)
+
     indent(tree.getroot(), space="\t", level=0)
     tree.write(save_xml_path, encoding="utf-8", xml_declaration=True)
+    tree.write(save_xml_path + "2", encoding="utf-8", xml_declaration=True)
 
 
 def parse_references(references) -> Dict[str, any]:

multiverse/resources/muv/panda_with_task_board.muv

@@ -13,10 +13,10 @@ simulations:
     simulator: mujoco
     world:
       name: world
-      path: work_table_with_buttons.xml
+      path: work_table.xml
     robots:
       panda:
-        path: mjcf/panda_with_vel_control.xml
+        path: mjcf/panda_ext.xml
         apply:
           body:
             panda:
@@ -26,12 +26,12 @@ simulations:
         disable_self_collision: "off"
         joint_state:
           joint1: 0.0
-          joint2: 0.0
+          joint2: -0.7853981633974483
           joint3: 0.0
-          joint4: -1.57079
+          joint4: -2.356194490192345
           joint5: 0.0
-          joint6: 1.57079
-          joint7: -0.7853
+          joint6: 1.5707963267948966
+          joint7: 0.7853981633974483
     objects:
       task_board:
         path: task_board.xml
@@ -51,60 +51,10 @@ simulations:
               quat: [1.0, 0.0, 0.0, 0.0]
         prefix:
           geom: task_board_tool_
-      left_hand:
-        path: left_hand.xml
-        apply:
-          body:
-            left_shadow_hand:
-              pos: [1.0, -0.25, 1.05]
-        prefix:
-          geom: left_
-      right_hand:
-        path: right_hand.xml
-        apply:
-          body:
-            right_shadow_hand:
-              pos: [1.0, 0.25, 1.05]
-        prefix:
-          geom: right_
     references:
-      left_shadow_hand:
-        body1: LeftHand_WristRoot
-        body2: lh_wrist_ref
-      lh_thdistal:
-        body1: LeftHand_ThumbTip
-        body2: lh_thdistal_ref
-      lh_ffdistal:
-        body1: LeftHand_IndexTip
-        body2: lh_ffdistal_ref
-      lh_mfdistal:
-        body1: LeftHand_MiddleTip
-        body2: lh_mfdistal_ref
-      lh_rfdistal:
-        body1: LeftHand_RingTip
-        body2: lh_rfdistal_ref
-      lh_lfdistal:
-        body1: LeftHand_PinkyTip
-        body2: lh_lfdistal_ref
-
-      right_shadow_hand:
-        body1: RightHand_WristRoot
-        body2: rh_wrist_ref
-      rh_thdistal:
-        body1: RightHand_ThumbTip
-        body2: rh_thdistal_ref
-      rh_ffdistal:
-        body1: RightHand_IndexTip
-        body2: rh_ffdistal_ref
-      rh_mfdistal:
-        body1: RightHand_MiddleTip
-        body2: rh_mfdistal_ref
-      rh_rfdistal:
-        body1: RightHand_RingTip
-        body2: rh_rfdistal_ref
-      rh_lfdistal:
-        body1: RightHand_PinkyTip
-        body2: rh_lfdistal_ref
+      hand:
+        body1: hand_ref
+        body2: hand
     config:
       max_time_step: 0.002
       min_time_step: 0.001
@@ -128,36 +78,8 @@ multiverse_clients:
           "joint_torque",
         ]
     receive:
-      # actuator1: ["cmd_joint_rvalue"]
-      # actuator2: ["cmd_joint_rvalue"]
-      # actuator3: ["cmd_joint_rvalue"]
-      # actuator4: ["cmd_joint_rvalue"]
-      # actuator5: ["cmd_joint_rvalue"]
-      # actuator6: ["cmd_joint_rvalue"]
-      # actuator7: ["cmd_joint_rvalue"]
-
-      actuator1: ["cmd_joint_angular_velocity"]
-      actuator2: ["cmd_joint_angular_velocity"]
-      actuator3: ["cmd_joint_angular_velocity"]
-      actuator4: ["cmd_joint_angular_velocity"]
-      actuator5: ["cmd_joint_angular_velocity"]
-      actuator6: ["cmd_joint_angular_velocity"]
-      actuator7: ["cmd_joint_angular_velocity"]
-
       actuator8: ["cmd_joint_tvalue"]
-
-      LeftHand_WristRoot: ["position", "quaternion"]
-      LeftHand_ThumbTip: ["position", "quaternion"]
-      LeftHand_IndexTip: ["position", "quaternion"]
-      LeftHand_MiddleTip: ["position", "quaternion"]
-      LeftHand_RingTip: ["position", "quaternion"]
-      LeftHand_PinkyTip: ["position", "quaternion"]
-      RightHand_WristRoot: ["position", "quaternion"]
-      RightHand_ThumbTip: ["position", "quaternion"]
-      RightHand_IndexTip: ["position", "quaternion"]
-      RightHand_MiddleTip: ["position", "quaternion"]
-      RightHand_RingTip: ["position", "quaternion"]
-      RightHand_PinkyTip: ["position", "quaternion"]
+      hand_ref: ["position", "quaternion"]
 
   ros:
     ros_nodes:
@@ -193,39 +115,3 @@ multiverse_clients:
       services:
         socket:
           - port: 7400
-
-    ros_control:
-      - meta_data:
-          world_name: world
-          length_unit: m
-          angle_unit: rad
-          mass_unit: kg
-          time_unit: s
-          handedness: rhs
-        port: 7601
-        controller_manager:
-          robot: panda
-          robot_description: /robot_description
-          urdf: panda/urdf/panda.urdf
-          config: panda/config/ros_control.yaml
-          actuators:
-            actuator1: joint1
-            actuator2: joint2
-            actuator3: joint3
-            actuator4: joint4
-            actuator5: joint5
-            actuator6: joint6
-            actuator7: joint7
-          controllers:
-            spawn:
-              - joint_state_controller
-                joint_trajectory_velocity_controller
-
-    ros_run:
-      rviz:
-        config: panda_with_task_board/config/rviz.rviz
-        robot_descriptions:
-          panda_description: panda/urdf/panda.urdf
-          gripper_2F_85_description: gripper-2F-85/urdf/gripper-2F-85.urdf
-          task_board_description: task_board/urdf/task_board.urdf
-          task_board_tool_description: task_board/urdf/task_board_tool.urdf

multiverse/resources/robots/franka_robotics/panda/mjcf/panda_ext.xml

@@ -25,7 +25,7 @@
   <default>
     <default class="panda">
       <material specular="0.5" shininess="0.25"/>
-      <joint armature="0.1" damping="10" axis="0 0 1" range="-2.8973 2.8973"/>
+      <joint armature="0.1" damping="100" axis="0 0 1" range="-2.8973 2.8973"/>
       <general dyntype="none" biastype="affine" ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>
       <default class="finger">
         <joint axis="0 1 0" type="slide" range="0 0.04"/>