ADVRHumanoids · IoannisDadiotis · Mar 21, 2022 · Mar 23, 2022 · Mar 24, 2022 · Oct 24, 2022
diff --git a/centauro_config/hal/centauro_gz.yaml b/centauro_config/hal/centauro_gz.yaml
@@ -16,3 +16,7 @@ xbotcore_devices:
         names: []
         thread: rt_main
         type: link_state_sensor
+
+    ft_gz:
+        names: []
+        thread: rt_main
diff --git a/centauro_mujoco/CMakeLists.txt b/centauro_mujoco/CMakeLists.txt
@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.0)
+project(centauro_mujoco)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+# install(DIRECTORY launch
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+# install(DIRECTORY worlds
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
diff --git a/centauro_mujoco/config/centauro.yaml b/centauro_mujoco/config/centauro.yaml
@@ -0,0 +1,65 @@
+motor_pd:
+  j_arm*_1: [500, 10]
+  j_arm*_2: [500, 10]
+  j_arm*_3: [500, 10]
+  j_arm*_4: [500, 10]
+  j_arm*_5: [100, 5]
+  j_arm*_6: [100, 5]
+  j_arm*_7: [100, 5]
+  hip_yaw_*: [3000, 30]
+  hip_pitch_*: [3000, 30]
+  knee_pitch_*: [3000, 30]
+  ankle_pitch_*: [1000, 10]
+  ankle_yaw_*: [300, 10]
+  neck_pitch: [10, 1]
+  neck_yaw: [10, 1]
+  torso_yaw: [1000, 30]
+  j_wheel_*: [0, 30]
+
+motor_vel:
+  j_wheel_*: [1]
+  neck_velodyne: [1]
+
+q_init:
+  hip_yaw_1: -0.746874
+  hip_pitch_1: -1.25409
+  knee_pitch_1: -1.55576
+  ankle_pitch_1: -0.301666
+  ankle_yaw_1: 0.746874
+  j_wheel_1: 0.0
+  hip_yaw_2: 0.746874
+  hip_pitch_2: 1.25409
+  knee_pitch_2: 1.55576
+  ankle_pitch_2: 0.301666
+  ankle_yaw_2: -0.746874
+  j_wheel_2: 0.0
+  j_arm1_1: 0.520149
+  j_arm1_2: 0.320865
+  j_arm1_3: 0.274669
+  j_arm1_4: -2.23604
+  j_arm1_5: 0.0500815
+  j_arm1_6: -0.781461
+  j_arm1_7: -0.0567608
+  neck_yaw: 0.0
+  neck_pitch: 0.2
+  hip_yaw_3: 0.746874
+  hip_pitch_3: 1.25409
+  knee_pitch_3: 1.55576
+  ankle_pitch_3: 0.301667
+  ankle_yaw_3: -0.746874
+  j_wheel_3: 0.0
+  hip_yaw_4: -0.746874
+  hip_pitch_4: -1.25409
+  knee_pitch_4: -1.55576
+  ankle_pitch_4: -0.301667
+  ankle_yaw_4: 0.746874
+  j_wheel_4: 0.0
+  j_arm2_1: 0.520149
+  j_arm2_2: -0.320865
+  j_arm2_3: -0.274669
+  j_arm2_4: -2.23604
+  j_arm2_5: -0.0500815
+  j_arm2_6: -0.781461
+  j_arm2_7: 0.0567608
+  torso_yaw: 0.0
+  neck_velodyne: 0.0
diff --git a/centauro_mujoco/config/options.xml b/centauro_mujoco/config/options.xml
@@ -0,0 +1,15 @@
+<mujoco>
+    <compiler angle="radian" />
+    <option timestep="0.0002"/>
+    <size njmax="1000" nconmax="200"/>
+    <contact>
+        <exclude name="hip_1_exclude" body1="pelvis" body2="hip2_1"/>
+        <exclude name="hip_2_exclude" body1="pelvis" body2="hip2_2"/>
+        <exclude name="hip_3_exclude" body1="pelvis" body2="hip2_3"/>
+        <exclude name="hip_4_exclude" body1="pelvis" body2="hip2_4"/>
+    </contact>
+    <sensor>
+        <accelerometer name="imu_link_acc" site="imu_link"/>
+        <gyro name="imu_link_gyro" site="imu_link"/>
+    </sensor>
+</mujoco>
diff --git a/centauro_mujoco/config/sites.xml b/centauro_mujoco/config/sites.xml
@@ -0,0 +1,5 @@
+<sites>
+    <body name="imu_link">
+        <site name="imu_link"/>
+    </body>
+</sites>
diff --git a/centauro_mujoco/config/world.xml b/centauro_mujoco/config/world.xml
@@ -0,0 +1,13 @@
+<mujoco>
+    <asset>
+        <texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
+        <texture name="body" type="cube" builtin="flat" mark="cross" width="127" height="1278" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
+        <material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
+        <texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
+        <material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
+    </asset>
+    <worldbody>
+        <geom name="floor" size="0 0 .05" pos="0 0 -1.0" type="plane" material="grid" condim="3"/>
+        <light name="spotlight" mode="targetbodycom" target="pelvis" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="0 -20 4" cutoff="10"/>
+    </worldbody>
+</mujoco>
diff --git a/centauro_mujoco/launch/centauro_world.launch b/centauro_mujoco/launch/centauro_world.launch
@@ -0,0 +1,13 @@
+<launch>
+
+
+  <arg name="world" default="$(find centauro_mujoco)/config/world.xml"/>
+  <arg name="simopt" default="$(find centauro_mujoco)/config/options.xml"/>
+  <arg name="sites" default="$(find centauro_mujoco)/config/sites.xml"/>
+  <arg name="ctrlcfg" default="$(find centauro_mujoco)/config/centauro.yaml"/>
+  <arg name="urdffile" default="$(find centauro_urdf)/urdf/centauro.urdf"/>
+
+  <node pkg="centauro_mujoco" type="centauro_mujoco.py" name="centauro_mujoco"
+    args="--name centauro --urdf $(arg urdffile) --world $(arg world) --simopt $(arg simopt) --ctrlcfg $(arg ctrlcfg) --sites $(arg sites)"/>
+
+</launch>
diff --git a/centauro_mujoco/package.xml b/centauro_mujoco/package.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>centauro_mujoco</name>
+  <version>0.0.0</version>
+  <description>The centauro_mujoco package</description>
+
+  <maintainer email="enrico@todo.todo">enrico</maintainer>
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+  </export>
+</package>
diff --git a/centauro_mujoco/src/centauro_mujoco.py b/centauro_mujoco/src/centauro_mujoco.py
@@ -0,0 +1,160 @@
+#!/usr/bin/env python3
+
+from ast import arg
+import subprocess
+import os 
+import shutil
+import argparse
+import rospkg
+from lxml import etree
+from copy import deepcopy
+import sys
+import rospy
+
+rospy.init_node('mujoco_sim')
+
+descr = """
+A wrapper aroung mujoco_simulator that assembles a set of separate files
+that describe the simulation into a sigle XML file that mujoco can understand.
+These files are:
+ - the robot urdf
+ - a simulation option file (e.g. step size, disabled collisions, ...)
+ - a world description (e.g. a ground plane, lights, ...)
+ - a configuration file for the decentralized controllers (e.g., default gains)
+"""
+parser = argparse.ArgumentParser(description=descr)
+parser.add_argument('--urdf', help='The path to the robot urdf file')
+parser.add_argument('--simopt', help='The path to an XML file containing simulator options')
+parser.add_argument('--world', help='The path to an XML file containing the world description')
+parser.add_argument('--ctrlcfg', help='The path to a YAML file containing decentralized control configuration')
+parser.add_argument('--sites', help='The path to an XML file containing additional sites for the model')
+parser.add_argument('--name', help='Unique robot name')
+args, _ = parser.parse_known_args()
+
+# handle to rospack system
+rospack = rospkg.RosPack()
+
+# utils
+def remove_comments(XML):
+    tree = etree.fromstring(XML)
+    etree.strip_tags(tree, etree.Comment)
+    robot = tree.xpath('/robot')[0]
+    mujoco = etree.Element("mujoco")
+    compiler = etree.Element("compiler")
+    compiler.attrib['fusestatic'] = 'false'
+    mujoco.append(compiler)
+    robot.append(mujoco)
+    return etree.tostring(tree)
+
+
+def treeMerge(a, b):
+
+    def inner(aparent, bparent):
+        print(f'processing {aparent.tag} vs {bparent.tag}')
+        for bchild in bparent:
+            print(f'..processing {bchild.tag}')
+            achild = aparent.xpath('./' + bchild.tag)
+            if achild and bchild.getchildren():
+                inner(achild[0], bchild)
+            else:
+                aparent.append(bchild)
+
+
+    res = deepcopy(a)
+    inner(res, b)
+    return res
+
+
+# useful paths
+urdf_path = args.urdf
+mj_xml_dir = f'/tmp/{args.name}_mujoco'
+mj_urdf_path = os.path.join(mj_xml_dir, f'{args.name}.urdf')
+mj_xml_path = os.path.join(mj_xml_dir, f'{args.name}.xml')
+mj_xml_path_orig = os.path.join(mj_xml_dir, f'{args.name}.orig.xml')
+
+# create directory
+shutil.rmtree(mj_xml_dir, ignore_errors=True)
+os.makedirs(mj_xml_dir, exist_ok=True)
+
+# pre-process urdf to 
+#  1) turn package:// directives into absolute paths
+#  2) create a different symlink for each mesh to circumvent mjc's bug
+with open(urdf_path, 'r') as file:
+    urdf = file.read()
+
+seq_id = 0
+urdf = remove_comments(urdf.encode()).decode()
+urdf_processed = str()
+last_pos = 0
+pos = urdf.find('filename="')    
+while pos != -1:
+    uri_start = pos + len('filename="')
+    uri_end = urdf.find('"', uri_start)
+    uri = urdf[uri_start:uri_end]
+
+    if uri.startswith('package://'):
+        pkg_start = len('package://')
+        pkg_end = uri.find('/', pkg_start)
+        pkg_name = uri[pkg_start:pkg_end]
+        uri = rospack.get_path(pkg_name) + uri[pkg_end:] 
+
+    filename = os.path.basename(uri)
+    dst_file = os.path.join(mj_xml_dir, str(seq_id) + '_' + filename)
+    seq_id += 1
+    if not os.path.exists(dst_file):
+        try: 
+            os.remove(dst_file)
+        except: pass
+
+        os.symlink(uri, dst_file)
+
+    urdf_processed = urdf_processed + urdf[last_pos:uri_start] + dst_file
+    last_pos = uri_end
+    pos = urdf.find('filename="', uri_end)    
+
+urdf_processed = urdf_processed + urdf[last_pos:]
+
+# write pre-processed urdf
+open(mj_urdf_path, 'w').write(urdf_processed)
+
+# produce mujoco's xml
+cmd = f'mujoco_compile {mj_urdf_path} {mj_xml_path_orig}'
+subprocess.run(cmd.split())
+
+# add options and world
+with open(mj_xml_path_orig, 'r') as file:
+    mj_xml = file.read()
+    mj_xml_tree = etree.fromstring(mj_xml)
+
+with open(args.simopt, 'r') as file:
+    mj_opt = file.read()
+    mj_opt_tree = etree.fromstring(mj_opt)
+
+with open(args.world, 'r') as file:
+    mj_world = file.read()
+    mj_world_tree = etree.fromstring(mj_world)
+
+with open(args.sites, 'r') as file:
+    mj_sites = file.read()
+    mj_sites_tree = etree.fromstring(mj_sites)
+
+
+mj_xml_tree.remove(mj_xml_tree.xpath('./compiler')[0])
+mj_xml_tree.remove(mj_xml_tree.xpath('./size')[0])
+
+xml_merged = treeMerge(mj_xml_tree, mj_opt_tree)
+xml_merged = treeMerge(xml_merged, mj_world_tree)
+
+# add sites
+site_bodies = mj_sites_tree.xpath('./body')
+for sb in site_bodies:
+    sname = sb.get('name')
+    body = xml_merged.findall(f".//body[@name='{sname}']")[0]
+    site = etree.Element('site')
+    site.attrib['name'] = sb.xpath('./site')[0].get('name')
+    body.append(site)
+
+open(mj_xml_path, 'w').write(etree.tostring(xml_merged, pretty_print=True).decode())
+subprocess.run(['mujoco_simulator', mj_xml_path, args.ctrlcfg])
+
+print('bye')