Merge branch 'develop' of https://github.com/shbang91/rpc into develop

shbang91 · Aug 22, 2024 · 3aa0f91 · 3aa0f91
2 parents 5f8d061 + c3ad6e6
commit 3aa0f91
Show file tree

Hide file tree

Showing 79 changed files with 199,387 additions and 76 deletions.
diff --git a/.github/workflows/linux.yml b/.github/workflows/linux.yml
@@ -74,8 +74,6 @@ jobs:
           export PYTHONPATH=/opt/openrobots/lib/python3.9/site-packages:$PYTHONPATH
           export CMAKE_PREFIX_PATH=/opt/openrobots:/usr/local/include:$CMAKE_PREFIX_PATH
           sudo apt-get install -y libglfw3 libglfw3-dev
-          ./dependency/scripts/install_zmq.sh
-          ./dependency/scripts/install_protobuf.sh
           pip install conan
           mkdir -p ~/.conan2/profiles/ && cp .github/conan_profile ~/.conan2/profiles/default
           conan install conanfile.txt --build=missing && cd build/ && cmake .. -DBUILD_WITH_ZMQ_PROTOBUF=ON -DBUILD_WITH_FOXGLOVE=ON && make
diff --git a/config/draco/sim/mujoco/ihwbc/mujoco_params.yaml b/config/draco/sim/mujoco/ihwbc/mujoco_params.yaml
@@ -78,7 +78,7 @@ actuator_gains:
     kp: 100
     kd: 5
   right_ezgripper_knuckle_palm_L1_1:
-    kp: 10
+    kp: 30
     kd: 5
 
 initial_config:

diff --git a/config/draco/sim/mujoco/ihwbc/pnc.yaml b/config/draco/sim/mujoco/ihwbc/pnc.yaml
@@ -23,7 +23,7 @@ contact_detection:
 
 
 state_estimator:
-  b_cheater_mode: false ## true: use simulation groundtruth states
+  b_cheater_mode: true ## true: use simulation groundtruth states
   state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf 
   b_use_marg_filter: false   # only used when kf is set to true
 
@@ -62,7 +62,7 @@ controller:
   b_smoothing_command: true 
   smoothing_command_duration: 1.0 ## this will filter the control command at the beginning
   b_use_modified_swing_foot_jac: true
-  b_use_modified_hand_jac: true
+  b_use_modified_hand_jac: false
   b_use_filtered_torque: false
   alpha_cmd: 0.5
 
@@ -110,7 +110,7 @@ wbc:
       kd : [0.0, 0.0, 2.0]
 
     torso_ori_task:
-      weight : [100., 100., 10.]
+      weight : [100., 100., 100.]
       kp : [200., 200., 100.]
       kd : [14., 14., 10.]
 
@@ -211,18 +211,18 @@ state_machine:
 
 
 dcm_walking:
-  #t_additional_init_trans: 0.
-  #t_contact_trans: 0.45
-  #t_swing: 0.55
-  #percentage_settle: 0.9
-  #alpha_ds: 0.5
-
-  t_additional_init_trans: 0.4
-  t_contact_trans: 1.2
-  t_swing: 0.8
+  t_additional_init_trans: 0.
+  t_contact_trans: 0.45
+  t_swing: 0.55
   percentage_settle: 0.9
   alpha_ds: 0.5
 
+  #t_additional_init_trans: 0.4
+  #t_contact_trans: 1.2
+  #t_swing: 0.8
+  #percentage_settle: 0.9
+  #alpha_ds: 0.5
+
   #t_additional_init_trans: 0.4
   #t_contact_trans: 1.6
   #t_swing: 0.8
@@ -234,5 +234,5 @@ dcm_walking:
   nominal_backward_step: -0.1
   nominal_turn_radians: 0.3
   nominal_strafe_distance: 0.05
-  n_steps: 1
+  n_steps: 3
   first_swing_leg: 1 ## 0: left leg ## 1: right leg
diff --git a/config/draco/sim/pybullet/ihwbc/pybullet_params.py b/config/draco/sim/pybullet/ihwbc/pybullet_params.py
@@ -18,6 +18,8 @@ class Config(object):
 
     VIDEO_RECORD = False
     RECORD_FREQ = 50
+    RENDER_WIDTH = 1920
+    RENDER_HEIGHT = 1080
 
     ##TODO:
     USE_MESHCAT = False

diff --git a/config/draco/sim/pybullet/wbic/pybullet_params.py b/config/draco/sim/pybullet/wbic/pybullet_params.py
@@ -16,8 +16,10 @@ class Config(object):
 
     MEASURE_COMPUTATION_TIME = False
 
-    VIDEO_RECORD = False
+    VIDEO_RECORD = True
     RECORD_FREQ = 50
+    RENDER_WIDTH = 1920
+    RENDER_HEIGHT = 1080
 
     ##TODO:
     USE_MESHCAT = False

diff --git a/controller/draco_controller/draco_state_machines/teleop_manipulation.cpp b/controller/draco_controller/draco_state_machines/teleop_manipulation.cpp
@@ -90,6 +90,7 @@ void TeleopManipulation::OneStep() {
 
         Eigen::Isometry3d torso_com_iso =
             robot_->GetLinkIsometry(draco_link::torso_com_link);
+        torso_com_iso.linear() = robot_->GetBodyYawRotationMatrix();
         target_iso.translation() =
             torso_com_iso.linear() *
                 (initial_torso_to_rh_iso_.translation() + rs_commands_->pos_) +

diff --git a/robot_model/draco/draco_latest_collisions_with_gripper.xml b/robot_model/draco/draco_latest_collisions_with_gripper.xml
@@ -1,11 +1,11 @@
 <mujoco model="draco">
   <compiler angle="radian" autolimits="true" eulerseq="zyx" meshdir="mujoco_meshes/"/>
   <option gravity="0 0 -9.81" timestep="0.00125" solver="Newton" tolerance="1e-12" impratio="60.0" iterations="10"/>
-  <!--<option gravity="0 0 -9.81" timestep="0.00125" solver="Newton" cone="elliptic" tolerance="1e-12" impratio="1.0" iterations="10"/>-->
+  <!--<option gravity="0 0 -9.81" timestep="0.00125" solver="Newton" cone="elliptic" tolerance="1e-12" impratio="40.0" iterations="10"/>-->
   <!--<option gravity="0 0 -9.81" timestep="0.00125" solver="Newton" cone="elliptic" tolerance="1e-5" jacobian="dense" noslip_iterations="30" noslip_tolerance="1e-5" iterations="50"/>-->
   <default>
       <geom contype="1" conaffinity="2" condim="6" margin="0.0005" solref="0.005 1" group="1"/>
-      <!--<geom contype="1" conaffinity="2" condim="6" group="1"/>-->
+      <!--<geom contype="1" conaffinity="2" condim="6" margin="0.0005" solref="0.01 1" group="1"/>-->
       <equality solref="0.0002 1" solimp="0.99 0.999 0.00001"/>
       <joint limited="true"/>
       <light castshadow="true" diffuse="0.8 0.8 0.8"/>

diff --git a/robot_model/draco/draco_manipulation_cup_shelving.xml b/robot_model/draco/draco_manipulation_cup_shelving.xml
diff --git a/robot_model/draco/draco_manipulation_scene.xml b/robot_model/draco/draco_manipulation_scene.xml
diff --git a/robot_model/objects/bread-visual.xml b/robot_model/objects/bread-visual.xml
@@ -0,0 +1,15 @@
+<mujoco model="bread-visual">
+  <asset>
+    <mesh file="meshes/bread.stl" name="bread_mesh" scale="0.8 0.8 0.8"/>
+  </asset>
+  <worldbody>
+    <body>
+      <body name="object">
+        <geom pos="0 0 0" mesh="bread_mesh" type="mesh" rgba="0.8 0.8 0.8 0.3"  conaffinity="0" contype="0"  group="1" mass="0.0001"/>
+      </body>
+      <site rgba="0 0 0 0" size="0.005" pos="0 0 -0.045" name="bottom_site"/>
+      <site rgba="0 0 0 0" size="0.005" pos="0 0 0.03" name="top_site"/>
+      <site rgba="0 0 0 0" size="0.005" pos="0.03 0.03 0" name="horizontal_radius_site"/>
+    </body>
+  </worldbody>
+</mujoco>
diff --git a/robot_model/objects/bread.xml b/robot_model/objects/bread.xml
@@ -0,0 +1,17 @@
+<mujoco model="bread">
+  <asset>
+    <mesh file="meshes/bread.stl" name="bread_mesh" scale="2 0.3 2"/>
+    <texture file="../textures/bread.png" type="2d" name="tex-bread" />
+    <material name="bread" reflectance="0.7" texrepeat="15 15" texture="tex-bread" texuniform="true"/>
+  </asset>
+  <worldbody>
+    <body>
+      <body name="object">
+        <geom pos="0 0 0" mesh="bread_mesh" type="mesh" solimp="0.998 0.998 0.001" solref="0.001 1" density="50" friction="0.95 0.3 0.1"  material="bread" group="0" condim="4"/>
+      </body>
+      <site rgba="0 0 0 0" size="0.005" pos="0 0 -0.045" name="bottom_site"/>
+      <site rgba="0 0 0 0" size="0.005" pos="0 0 0.03" name="top_site"/>
+      <site rgba="0 0 0 0" size="0.005" pos="0.03 0.03 0" name="horizontal_radius_site"/>
+    </body>
+  </worldbody>
+</mujoco>
diff --git a/robot_model/objects/brush.xml b/robot_model/objects/brush.xml
@@ -0,0 +1,149 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- from https://github.com/vikashplus/mj_envs/tree/master/mj_envs/hand_manipulation_suite/assets -->
+<!-- Information about door can be found in this paper: https://arxiv.org/pdf/1709.10087.pdf -->
+
+<mujoco model="brush">
+  <asset>
+    <texture file="../textures/light-wood.png" type="cube" name="light-wood" />
+    <texture file="../textures/dark-wood.png" type="cube" name="dark-wood" />
+    <material name="MatDarkWood" texture="dark-wood" texrepeat="3 3" specular="0.4" shininess="0.1" />
+    <material name="MatLightWood" texture="light-wood" texrepeat="3 3" specular="0.4" shininess="0.1" />
+  </asset>
+    <worldbody>
+      <body>
+        <body name="object">
+          <site name="origin" pos="0 0 0" rgba="0 0 0 0" size="0.005"/>
+
+          <body name="body" pos="0 0 0.02">          
+            <geom pos="0.0 -0.02 0.0" size="0.05 0.02 0.02" type="box" friction="1 1 1" group="0" name="body" material="MatLightWood"/>
+            <geom pos="0 0.05 0.0" size="0.006 0.05 0.01" type="box" friction="1 1 1" group="0" name="handle" material="MatLightWood"/>
+
+            <site name="handle" pos="0 0.05 0" rgba="0 0 0 0" size="0.02"/>
+
+            <inertial pos="0. 0 0" quat="0.5 0.5 0.5 0.5" mass="0.01"  diaginertia="0.0001 0.0001 0.0001" />
+            <body name="furs" pos="0 -0.04 0">
+
+              <joint name="joint_brush"
+                  type="ball"
+                  stiffness="5.0" springref="0.0" damping="0.01"
+                  />
+              <inertial pos="0. 0 0" mass="0.01"  diaginertia="0.0001 0.0001 0.0001" />
+
+            <body name="fur_row1" pos="-0.04 0 0">
+              <body pos="0 0 -0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>            
+            </body>
+
+            <body name="fur_row2" pos="-0.03 0 0">
+              <body pos="0 0 -0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+            </body>
+
+            <body name="fur_row3" pos="-0.02 0 0">
+              <body pos="0 0 -0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+            </body>
+
+            <body name="fur_row4" pos="-0.01 0 0">
+              <body pos="0 0 -0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+            </body>
+
+            <body name="fur_row5" pos="0 0 0">
+              <body pos="0 0 -0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+            </body>
+
+            <body name="fur_row6" pos="0.01 0 0">
+              <body pos="0 0 -0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+            </body>
+
+            <body name="fur_row7" pos="0.02 0 0">
+              <body pos="0 0 -0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+            </body>
+
+            <body name="fur_row8" pos="0.03 0 0">
+              <body pos="0 0 -0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+            </body>
+
+            <body name="fur_row9" pos="0.04 0 0">
+              <body pos="0 0 -0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+              <body pos="0 0 0.01">
+                <geom fromto="0 0 0 0 -0.05 0" size="0.003" type="cylinder" group="0" material="MatDarkWood"/>
+              </body>
+            </body>
+
+            </body>
+          </body>
+
+        </body>
+        <site name="bottom_site" pos="0 0 -0.6" rgba="0 0 0 0" size="0.005"/>
+        <site name="top_site" pos="0 0 0.6" rgba="0 0 0 0" size="0.005"/>
+        <site name="horizontal_radius_site" pos="0.3 0 0" rgba="0 0 0 0" size="0.1"/>
+      </body>
+    </worldbody>
+</mujoco>
-Original file line number
+Diff line change
@@ Expand Up / @@ -78,7 +78,7 @@ actuator_gains: @@
         kp: 100
         kd: 5
       right_ezgripper_knuckle_palm_L1_1:
-        kp: 10
+        kp: 30
         kd: 5
     initial_config:
@@ Expand Down @@