diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index 48640af3..e9a4731e 100644
--- a/.github/workflows/main.yml
+++ b/.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:
diff --git a/load_vcs_workspace.sh b/load_vcs_workspace.sh
index 92033647..85ba4aef 100755
--- a/load_vcs_workspace.sh
+++ b/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
diff --git a/marimbabot_description/urdf/two_mallet_compliance_holder.urdf.xacro b/marimbabot_description/urdf/two_mallet_compliance_holder.urdf.xacro
index 45bea189..31e0ed52 100644
--- a/marimbabot_description/urdf/two_mallet_compliance_holder.urdf.xacro
+++ b/marimbabot_description/urdf/two_mallet_compliance_holder.urdf.xacro
@@ -23,6 +23,11 @@
+
+
+
+
+
@@ -46,6 +51,17 @@
+
+
+
+
+
+
+
+
+
+
+
@@ -65,6 +81,11 @@
+
+
+
+
+
@@ -84,6 +105,11 @@
+
+
+
+
+
@@ -94,6 +120,11 @@
+
+
+
+
+
@@ -104,17 +135,26 @@
+
+
+
+
+
-
+
+
+
+
+
@@ -125,6 +165,11 @@
+
+
+
+
+
@@ -147,10 +192,20 @@
-
+
+
+ transmission_interface/SimpleTransmission
+
+ 1.0
+
+
+ hardware_interface/EffortJointInterface
+
+
+
@@ -180,5 +235,4 @@
-
\ No newline at end of file
diff --git a/marimbabot_hardware/scripts/joint_state.py b/marimbabot_hardware/scripts/joint_state.py
index e18eb792..bc1d62da 100755
--- a/marimbabot_hardware/scripts/joint_state.py
+++ b/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
\ No newline at end of file
+ pass
diff --git a/marimbabot_simulation/README.md b/marimbabot_simulation/README.md
index 532311a3..ac65e2cf 100644
--- a/marimbabot_simulation/README.md
+++ b/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/.midi -a alsa
-```
\ No newline at end of file
+fluidsynth /usr/share/sounds/sf2/FluidR3_GM.sf2 .midi -a alsa
+```
+
+In the above command, replace the path and `` 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 .midi | twolame -b 256 -r - .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
\ No newline at end of file
diff --git a/marimbabot_simulation/launch/marimbabot_gazebo.launch b/marimbabot_simulation/launch/marimbabot_gazebo.launch
index 76dff31d..23ff13be 100644
--- a/marimbabot_simulation/launch/marimbabot_gazebo.launch
+++ b/marimbabot_simulation/launch/marimbabot_gazebo.launch
@@ -49,7 +49,13 @@
args="gripper_controller" respawn="false" output="screen" />
- -->
+
+
+
+
+
+
-
-
+
+
/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_/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"
diff --git a/marimbabot_ur5_flex_double_moveit_config/config/gripper_controller.yaml b/marimbabot_ur5_flex_double_moveit_config/config/gripper_controller.yaml
index 7100f2d4..d73ec073 100644
--- a/marimbabot_ur5_flex_double_moveit_config/config/gripper_controller.yaml
+++ b/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
diff --git a/marimbabot_ur5_flex_double_moveit_config/config/mallet_finger_controller.yaml b/marimbabot_ur5_flex_double_moveit_config/config/mallet_finger_controller.yaml
new file mode 100644
index 00000000..88a732c5
--- /dev/null
+++ b/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
diff --git a/marimbabot_ur5_flex_double_moveit_config/config/marimbabot_ur5.srdf b/marimbabot_ur5_flex_double_moveit_config/config/marimbabot_ur5.srdf
index 3eeac725..e32b2db2 100644
--- a/marimbabot_ur5_flex_double_moveit_config/config/marimbabot_ur5.srdf
+++ b/marimbabot_ur5_flex_double_moveit_config/config/marimbabot_ur5.srdf
@@ -2033,4 +2033,5 @@
+