Make simulation work with double mallet setup

.github/workflows/main.yml

@@ -13,7 +13,7 @@ jobs:
         env:
           ROS_DISTRO: noetic
           ROS_REPO: main
-          AFTER_SETUP_TARGET_WORKSPACE: /root/target_ws/src/marimbabot/load_vcs_workspace.sh
+          AFTER_SETUP_TARGET_WORKSPACE: /root/target_ws/src/MarimbaBot/load_vcs_workspace.sh
   build_python_requirements:
     runs-on: ubuntu-20.04
     steps:

load_vcs_workspace.sh

@@ -10,8 +10,8 @@ curl -s https://packagecloud.io/install/repositories/dirk-thomas/vcstool/script.
 sudo apt install -y python3-vcstool
 
 # setup git so it does not fail due to missmatching owners during vcs import
-git config --global --add safe.directory /root/target_ws/src/marimbabot
+git config --global --add safe.directory /root/target_ws/src/MarimbaBot
 
 # import workspace.repos file
 cd /root/target_ws/src
-vcs import --skip-existing --recursive < marimbabot/workspace.repos
+vcs import --skip-existing --recursive < MarimbaBot/workspace.repos

marimbabot_description/urdf/two_mallet_compliance_holder.urdf.xacro

@@ -23,6 +23,11 @@
                 <box size="0.077 ${(68.4+6+6)/1000} 0.11"/>
             </geometry>
         </collision>
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <mass value="0.01"/>
+            <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+        </inertial>
     </link>
 
     <!-- Mallet Battery Collision Object for the battery location -->
@@ -46,6 +51,17 @@
                 <color rgba="1.0 1.0 1.0 1.0"/>
             </material>
         </visual>
+        <collision>
+            <origin xyz="${-(10)/1000} ${(6-(68.4+6+6)/2)/1000} ${(13.3+10)/1000}" rpy="${-pi / 2} 0.0 0.0"/>
+            <geometry>
+                <mesh filename="package://marimbabot_description/mesh/two_mallet_servo_element.stl" scale="0.001 0.001 0.001"/>
+            </geometry>
+        </collision>
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <mass value="0.01"/>
+            <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+        </inertial>
     </link>
 
     <!-- Mallet Base Box for the first, fixed mallet -->
@@ -65,6 +81,11 @@
                 <box size="0.033 0.09 0.09"/>
             </geometry>
         </collision>
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <mass value="0.01"/>
+            <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+        </inertial>
     </link>
 
     <!-- Mallet Base Box for the second, servo controlled mallet -->
@@ -84,6 +105,11 @@
                 <box size="0.033 0.09 0.09"/>
             </geometry>
         </collision>
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <mass value="0.01"/>
+            <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+        </inertial>
     </link>
 
     <!-- Mallet Neck for the first, fixed mallet -->
@@ -94,6 +120,11 @@
                 <cylinder radius="0.0042" length="0.375"/> 
             </geometry>
         </collision>
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <mass value="0.01"/>
+            <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+        </inertial>
     </link>
 
     <!-- Mallet Neck for the second, servo controlled mallet -->
@@ -104,17 +135,26 @@
                 <cylinder radius="0.0042" length="0.375"/> 
             </geometry>
         </collision>
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <mass value="0.01"/>
+            <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+        </inertial>
     </link>
 
     <!-- Mallet Top for the first, fixed mallet -->
     <link name="mallet_head_1">
         <collision>
             <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
-
             <geometry>
                 <sphere radius="0.018375"/>
             </geometry>
         </collision>
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <mass value="0.01"/>
+            <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+        </inertial>
     </link>
 
     <!-- Mallet Top for the second, servo controlled mallet -->
@@ -125,6 +165,11 @@
                 <sphere radius="0.018375"/>
             </geometry>
         </collision>
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <mass value="0.01"/>
+            <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+        </inertial>
     </link>
 
     <!-- Joint definitions -->
@@ -147,10 +192,20 @@
         <parent link="mallet_base_link"/>
         <child link="mallet_servo_box"/>
         <origin xyz="${(-77/2+10+7)/1000} 0.0 ${0.0368+(110-60)/1000}"/>
-        <limit lower="0.0" upper="1.22" effort="1.0" velocity="7.0"/>
+        <limit lower="0.05" upper="1.22" effort="2.0" velocity="7.0"/>
         <axis xyz="0.0 -1.0 0.0"/>
     </joint>
 
+    <transmission name="mallet_finger_transmission">
+      <type>transmission_interface/SimpleTransmission</type>
+      <actuator name="mallet_finger_motor">
+        <mechanicalReduction>1.0</mechanicalReduction>
+      </actuator>
+      <joint name="mallet_finger">
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      </joint>
+    </transmission>
+
     <joint name="mallet_group_1_start_link" type="fixed">
         <parent link="mallet_base_box_1"/>
         <child link="mallet_neck_1"/>
@@ -180,5 +235,4 @@
         <child link="mallet_head_2"/>
         <origin xyz="0.0 0.0 ${0.365+0.018375}"/>
     </joint>
-
 </robot>

marimbabot_hardware/scripts/joint_state.py

@@ -4,23 +4,42 @@
 
 import rospy
 from sensor_msgs.msg import JointState
+from moveit_msgs.msg import MoveGroupActionGoal
+from std_msgs.msg import Header
+
 
 def JointStatePublisher():
-    pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
-    rospy.init_node('joint_state_publisher')
-    rate = rospy.Rate(100) # 100hz
-    Joint_state_pub = JointState()
-    Joint_state_pub.name.append("mallet_finger")
-    Joint_state_pub.position.append(0.0)
-    pub.publish(Joint_state_pub)
+    pub = rospy.Publisher("/joint_states", JointState, queue_size=10)
+    rospy.init_node("joint_state_publisher")
+    rate = rospy.Rate(100)  # 100hz
 
     while not rospy.is_shutdown():
-        Joint_state_pub.header.stamp = rospy.Time.now()
-        pub.publish(Joint_state_pub)
+        # TODO: get the start position of the mallet_finger joint instead of goal position
+        goal_msg: MoveGroupActionGoal = rospy.wait_for_message("/move_group/goal", MoveGroupActionGoal)
+
+        constraints = goal_msg.goal.request.goal_constraints
+
+        mallet_finger_position = None
+        for constraint in constraints:
+            for joint_constraint in constraint.joint_constraints:
+                if joint_constraint.joint_name == "mallet_finger":
+                    mallet_finger_position = joint_constraint.position
+                    break
+
+        if mallet_finger_position is not None:
+            Joint_state_pub = JointState()
+            Joint_state_pub.header = Header()
+            Joint_state_pub.header.stamp = rospy.Time.now()
+            Joint_state_pub.name.append("mallet_finger")
+            Joint_state_pub.position.append(mallet_finger_position)
+            # Joint_state_pub.position.append(0.0)
+            pub.publish(Joint_state_pub)
+
         rate.sleep()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     try:
         JointStatePublisher()
     except rospy.ROSInterruptException:
-        pass
+        pass

marimbabot_simulation/README.md

@@ -1,25 +1,59 @@
 # Simulation package
 
+
+## Overview
+
+The simulation consists of a Gazebo world with a Marimbabot model, a node to detect contacts and generate MIDI, and optionally PlotJuggler to view contact velocities. The planning node is launched as well to allow the user to play the simulation using the action server. 
+
+
 ## Launch files
 
 There are the following launch files:
 
 * `marimbabot_gazebo.launch`: Launches the simulation in Gazebo.
-* `marimbabot_full_sim.launch`: Launches the simulation in Gazebo, as well as nodes to detect contacts and publish the contact points, and generate MIDI.
+* `marimbabot_full_sim.launch`: Launches the simulation in Gazebo, as well as nodes to detect contacts, publish the contact points, generate MIDI, and optionally launch PlotJuggler to view contact velocities.
+  *This is the recommended launch file to use.*
+
+The PlotJuggler can be toggled on/off by setting the appropriate bool flag in the if block in the 
+[the launch file](launch/marimbabot_full_sim.launch).
 
 
 ## How to run
 
+First, make sure you have completed the steps in the [main README](../README.md) and therefore have a working environment.
+
 Launch the simulation with the following command:
 
 ```bash
 roslaunch marimbabot_simulation marimbabot_full_sim.launch
 ```
 
-The MIDI file will be saved in the `marimbabot_simulation/midi` package folder with current datetime as name.
+The simulation can play any piece, and therefore needs to be fed a note string either manually using the action server, or using the `sound_pad.py` [script](../marimbabot_planning/scripts/sound_pad.py) in the `marimbabot_planning/scripts` package folder. E.g. to launch the Sound Pad script:
+
+```bash
+rosrun marimbabot_planning sound_pad.py
+```
+
+
+## MIDI generation
+
+The simulation will record all bar hits (starting from the first hit to avoid a long silent interval in the beginning) and save them as a MIDI file in the `marimbabot_simulation/midi` package folder with current datetime as name. If the `marimbabot_simulation` directory cannot be found using `rospack find`, then the script falls back to the current directory. In any case, the MIDI directory will be logged to the output stream.
 
 The MIDI file can be played as follows:
 
 ```bash
-fluidsynth /usr/share/sounds/sf2/FluidR3_GM.sf2 catkin_ws/src/marimbabot/marimbabot_simulation/midi/<FILENAME>.midi -a alsa
-```
+fluidsynth /usr/share/sounds/sf2/FluidR3_GM.sf2 <PATH/TO/MIDI_FILE>.midi -a alsa
+```
+
+In the above command, replace the path and `<FILENAME>` with the name of the MIDI file.
+
+To convert a MIDI file to MP3, use the following command template:
+
+```bash
+fluidsynth -l -T raw -F - /usr/share/sounds/sf2/FluidR3_GM.sf2 <PATH/TO/MIDI_FILE>.midi | twolame -b 256 -r - <NAME>.mp3 
+```
+
+
+# TODO
+- [ ] Try tilting the mallet holder to make sure the second mallet hits the bar
+- [ ] detect_bar_contact: fix 2 notes hits at the same time

marimbabot_simulation/launch/marimbabot_gazebo.launch

@@ -49,7 +49,13 @@
         args="gripper_controller" respawn="false" output="screen" />
 
   <!-- we want to remap the gripper controller topics, but that is not so easy with gazebo plugins... -->
-  <remap from="/gripper_action" to="/gripper_controller/follow_joint_trajectory" /> -->
+  <remap from="/gripper_action" to="/gripper_controller/follow_joint_trajectory" />
+
+  <!-- Mallet finger controller -->
+  <rosparam file="$(find marimbabot_ur5_flex_double_moveit_config)/config/mallet_finger_controller.yaml" command="load"/>
+
+  <node name="mallet_finger_controller_spawner" pkg="controller_manager" type="spawner" 
+        args="MalletHolder/trajectory_controller" respawn="false" output="screen" />
 
   <!-- fake UR5 calibration -->
   <node pkg="rostopic" type="rostopic" name="fake_joint_calibration"
@@ -59,8 +65,8 @@
   <include file="$(find marimbabot_ur5_flex_double_moveit_config)/launch/move_group.launch"/>
 
   <!-- Launch finger mallet faker -->
-  <node name="joint_state_mallet_finger" pkg="marimbabot_hardware" type="joint_state.py" output="screen"/>
-  <include file="$(find marimbabot_hardware)/launch/mallet_servo_control_debug_dummy.launch"/>
+  <!-- <node name="joint_state_mallet_finger" pkg="marimbabot_hardware" type="joint_state.py" output="screen"/> -->
+  <!-- <include file="$(find marimbabot_hardware)/launch/mallet_servo_control_debug_dummy.launch"/> -->
 
   <group if="$(arg launch_rviz)">
    <node name="rviz" pkg="rviz" type="rviz" respawn="false"

marimbabot_simulation/src/detect_bar_contact.py

@@ -2,9 +2,8 @@
 
 #!/usr/bin/env python
 
-import os
-import subprocess
 import datetime
+import subprocess
 from pathlib import Path
 
 import rospy
@@ -13,29 +12,24 @@
 from std_msgs.msg import String
 
 
-def find_file_directory(filename):
-    home_directory = os.path.expanduser("~")
-    command = f"find {home_directory} -type f -xtype f -name {filename} -exec dirname {{}} \\; -quit 2>/dev/null"
-    try:
-        output = subprocess.check_output(command, shell=True, universal_newlines=True)
-        directory = output.strip()
-        if directory:
-            return directory
-        else:
-            return None
-    except subprocess.CalledProcessError:
-        return None
-
-
 # velocity threshold to count as a bar hit
-VEL_THRESHOLD = 0.01
+VEL_THRESHOLD = 0.005
 
 # time between contacts to register as a separate one
-TIME_THRESHOLD = 1.0  # s
+TIME_THRESHOLD = 0.0  # s
 
 # duration of a note (assume fixed for now)
 NOTE_DURATION = 0.5  # s
 
+
+# where to write MIDI files
+try:
+    MIDI_DIR = subprocess.check_output(["rospack", "find", "marimbabot_simulation"]).decode("utf-8").strip()
+    rospy.loginfo(f"MIDI dir set to {MIDI_DIR}")
+except subprocess.CalledProcessError:
+    rospy.logerr("Could not find MIDI directory. Writing to current directory instead.")
+    MIDI_DIR = "./"
+
 # names of bars we are interested in
 # corresponding joints are bar_<note>/joint
 # e.g. "bar_a4" <-> "bar_a4/joint"
@@ -91,8 +85,7 @@ def find_file_directory(filename):
 def save_midi(notes):
     """Save notes to a MIDI file using pretty_midi."""
 
-    src_dir = find_file_directory("detect_bar_contact.py")
-    midi_dir = Path(src_dir).parent / "midi"
+    midi_dir = Path(MIDI_DIR) / "midi"
     midi_dir.mkdir(exist_ok=True)
     midi_path = midi_dir / f"{datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S')}.midi"
 

marimbabot_ur5_flex_double_moveit_config/config/gripper_controller.yaml

@@ -50,7 +50,7 @@ gripper_controller:
     s_model_finger_middle_joint_1: {trajectory: 0.1, goal: 0.01}
     s_model_finger_middle_joint_2: {trajectory: 0.1, goal: 0.01}
     s_model_finger_middle_joint_3: {trajectory: 0.1, goal: 0.01}
-  stop_trajctory_duration: 0.5
+  stop_trajectory_duration: 0.5
   state_publish_rate: 100
   action_monitor_rate: 20
 

marimbabot_ur5_flex_double_moveit_config/config/mallet_finger_controller.yaml

@@ -0,0 +1,13 @@
+MalletHolder/trajectory_controller:
+  type: effort_controllers/JointTrajectoryController
+  joints:
+    - mallet_finger
+  gains:
+    mallet_finger:      { p: 100.0, d:  0.0, i: 10.0, i_clamp: 20.0 }
+  constraints:
+    goal_time: 0.6
+    stopped_velocity_tolerance: 0.05
+    mallet_finger: {trajectory: 0.1, goal: 0.01}
+  stop_trajectory_duration: 0.5
+  state_publish_rate: 100
+  action_monitor_rate: 20

marimbabot_ur5_flex_double_moveit_config/config/marimbabot_ur5.srdf

@@ -2033,4 +2033,5 @@
     <disable_collisions link1="bar_gis4" link2="mallet_head_2" reason="User"/>
     <disable_collisions link1="bar_gis5" link2="mallet_head_2" reason="User"/>
     <disable_collisions link1="bar_gis6" link2="mallet_head_2" reason="User"/>
+    <disable_collisions link1="mallet_base_link" link2="mallet_servo_box" reason="User"/>
 </robot>