Merge pull request #283 from UHHRobotics22-23/use_lower_notes_motion

Use the lower notes for sharps

marimbabot_audio/src/onset_detection.py

@@ -489,6 +489,6 @@ def publish_cqt(self, cqt):
 		self.pub_cqt.publish(msg)
 
 if __name__ == '__main__':
-	rospy.init_node("detect_onset",log_level=rospy.DEBUG)
+	rospy.init_node("detect_onset")
 	detector = OnsetDetection()
 	detector.start()

marimbabot_audio/src/sequence_evaluation.py

@@ -157,7 +157,7 @@ def run(self):
 
 
 if __name__ == '__main__':
-	rospy.init_node('score_calculator', log_level=rospy.DEBUG)
+	rospy.init_node('score_calculator')
 	window_size = rospy.get_param("~window_size", 1)  # sec
 	compare = Comparison(windows_t_for_compare=1)
 	compare.run()

marimbabot_hardware/config/controllers.yaml

@@ -9,8 +9,8 @@ trajectory_controller:
     - mallet_finger
   constraints:
     mallet_finger:
-      trajectory: 0.3
-      goal: 0.1
+      trajectory: 0.5
+      goal: 0.5
 
 position_controller:
   type: "position_controllers/JointPositionController"

marimbabot_planning/include/marimbabot_planning/planning.h

@@ -41,6 +41,9 @@ class Planning
         // Last action time
         ros::Time last_action_time_;
 
+        // Flag that indicates that the robot is currently playing
+        bool is_playing_ = false;
+
         /**
          * @brief Move the robot to its idle/home position
          * 
@@ -94,10 +97,14 @@ class Planning
          * @brief Callback for the action server
          * 
          * @param goal
-         * @param action_server
          */
         void action_server_callback(const marimbabot_msgs::HitSequenceGoalConstPtr &goal);
 
+        /**
+         * Callback for the action server preemtion (cancel)
+         */
+        void preempt_action();
+
     public:
         Planning(const std::string planning_group);
 };

marimbabot_planning/include/marimbabot_planning/utils.h

@@ -1,3 +1,4 @@
+#include <boost/algorithm/string.hpp>
 #include <boost/optional.hpp>
 #include <geometry_msgs/PointStamped.h>
 #include <marimbabot_msgs/HitSequenceAction.h>

marimbabot_planning/src/planning.cpp

@@ -34,6 +34,8 @@ Planning::Planning(const std::string planning_group) :
     };
     // Create timer that checks if an action is active and moves back to the home position if not
     auto timer = nh_.createTimer(ros::Duration(0.5), timer_callback);
+    // Set preempt callback that is executed when the action is canceled
+    action_server_.registerPreemptCallback(boost::bind(&Planning::preempt_action, this));
     // Start action server
     action_server_.start();
     // Wait for shutdown
@@ -299,7 +301,6 @@ std::pair<moveit::planning_interface::MoveGroupInterface::Plan, ros::Duration> P
 /**
  * Callback for the action server
  * @param goal
- * @param action_server
  */
 void Planning::action_server_callback(const marimbabot_msgs::HitSequenceGoalConstPtr &goal)
 {
@@ -311,6 +312,9 @@ void Planning::action_server_callback(const marimbabot_msgs::HitSequenceGoalCons
     marimbabot_msgs::HitSequenceResult result;
 
     try {
+        // Cancel all current moveit plans
+        move_group_interface_.stop();
+
         // Set the max velocity and acceleration scaling factors
         move_group_interface_.setMaxVelocityScalingFactor(0.95);
         move_group_interface_.setMaxAccelerationScalingFactor(0.95);
@@ -352,27 +356,28 @@ void Planning::action_server_callback(const marimbabot_msgs::HitSequenceGoalCons
         ros::Time first_note_hit_time = start_time + approach_time_to_first_note_hit;
         result.first_note_hit_time = first_note_hit_time;
 
+        // Check if the action has been preempted in the meantime
+        if (action_server_.isPreemptRequested()) {
+            action_server_.setPreempted();
+            return;
+        }
+
         // Publish the feedback
         feedback.playing = true;
         action_server_.publishFeedback(feedback);
 
         // Execute the plan
-        auto status = move_group_interface_.execute(hit_plan);
+        is_playing_ = true;
+        move_group_interface_.execute(hit_plan);
+        is_playing_ = false;
 
         // Set playing to false
         feedback.playing = false;
         action_server_.publishFeedback(feedback);
 
         // Set the result of the action server
-        if (status != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
-            ROS_ERROR_STREAM("Hit sequence execution failed: " << status);
-            result.success = false;
-            result.error_code = marimbabot_msgs::HitSequenceResult::EXECUTION_FAILED;
-            action_server_.setAborted(result);
-        } else {
-            result.success = true;
-            action_server_.setSucceeded(result);
-        }
+        result.success = true;
+        action_server_.setSucceeded(result);
 
     } catch (PlanFailedException& e) {
         ROS_ERROR_STREAM("Hit sequence planning failed: " << e.what());
@@ -385,6 +390,79 @@ void Planning::action_server_callback(const marimbabot_msgs::HitSequenceGoalCons
     last_action_time_ = ros::Time::now();
 }
 
+/**
+ * Callback for the action server preemtion (cancel)
+ */
+void Planning::preempt_action()
+{
+    // Log to terminal
+    ROS_INFO("Preempting current plan");
+
+    if(is_playing_)
+    {
+        // Cancel current plan
+        move_group_interface_.stop();
+        is_playing_ = false;
+
+        // Set start state
+        moveit_msgs::RobotState start_state;
+        auto current_state = move_group_interface_.getCurrentState();
+        moveit::core::robotStateToRobotStateMsg(*current_state, start_state);
+        move_group_interface_.setStartState(start_state);
+
+        // Create robot state
+        moveit::core::RobotState robot_state(move_group_interface_.getRobotModel());
+        robot_state.setToDefaultValues();
+        robot_state.setVariablePositions(start_state.joint_state.name, start_state.joint_state.position);
+
+        // Use bio_ik to solve the inverse kinematics at the goal point
+        bio_ik::BioIKKinematicsQueryOptions ik_options;
+        ik_options.replace = true;
+        ik_options.return_approximate_solution = false; // Activate for debugging if you get an error 
+
+        // Get the current wrist height using tf
+        geometry_msgs::Transform wrist_location;
+        try
+        {
+            wrist_location = tf_buffer_->lookupTransform(
+                move_group_interface_.getPlanningFrame(),
+                "ur5_wrist_1_link",
+                ros::Time(0)).transform;
+        }
+        catch (tf2::TransformException &ex)
+        {
+            ROS_WARN("%s", ex.what());
+            return;
+        }
+
+        // Add link on plane constraint to ik_options that holds the wrist at the same height
+        ik_options.goals.emplace_back(new bio_ik::PlaneGoal(
+            "ur5_wrist_1_link", 
+            tf2::Vector3(0.0, 0.0, wrist_location.translation.z + 0.05),
+            tf2::Vector3(0.0, 0.0, 1.0)));
+
+        // Create minimal displacement goal, so that the robot does not move too much and stays close to the start state
+        ik_options.goals.emplace_back(new bio_ik::MinimalDisplacementGoal());
+
+        // Create dummy goal pose
+        geometry_msgs::Pose dummy_goal_pose;
+
+        robot_state.setFromIK(
+            move_group_interface_.getRobotModel()->getJointModelGroup(move_group_interface_.getName()),
+            dummy_goal_pose /* this is ignored with replace = true */,
+            0.0,
+            moveit::core::GroupStateValidityCallbackFn(),
+            ik_options);
+
+        // Plan to the goal state (but in joint space)
+        move_group_interface_.setJointValueTarget(robot_state);
+
+        // Execute plan
+        move_group_interface_.move();
+    }
+    action_server_.setPreempted();
+}
+
 } // namespace marimbabot_planning
 
 int main(int argc, char **argv)

marimbabot_planning/src/utils.cpp

@@ -79,7 +79,7 @@ moveit::planning_interface::MoveGroupInterface::Plan slow_down_plan(
     double original_length = input_plan.trajectory_.joint_trajectory.points.back().time_from_start.toSec();
     // Assert that the input plan is shorter than the desired length
     assert(original_length <= length && "Input plan must be shorter than the desired length");
-    
+
     // Calculate the scaling factor
     double scaling_factor = length / original_length;
 
@@ -95,7 +95,7 @@ moveit::planning_interface::MoveGroupInterface::Plan slow_down_plan(
     // Interpolate trajectory
     output_plan = interpolate_plan(output_plan, 100);
 
-    return output_plan;   
+    return output_plan;
 }
 
 
@@ -170,8 +170,8 @@ moveit::planning_interface::MoveGroupInterface::Plan interpolate_plan(
 
 
 /**
- * @brief Convert a hit sequence elements to a vector of HitSequenceElement structs containing the points 
- * 
+ * @brief Convert a hit sequence elements to a vector of HitSequenceElement structs containing the points
+ *
  * @param hit_sequence
  * @param tf_buffer
  * @param planning_frame
@@ -191,6 +191,19 @@ std::vector<CartesianHitSequenceElement> hit_sequence_to_points(
         // Map the note letter and octave to the corresponding tf frame
         std::string note_frame = "bar_" + point.tone_name + std::to_string(point.octave);
 
+        // Use the lower frames so we have less travel time for black keys
+        bool use_lower_frames = true;
+        if (use_lower_frames)
+        {
+            // Check if we have a flat or sharp note
+            if (boost::algorithm::ends_with(point.tone_name, "is") || boost::algorithm::ends_with(point.tone_name, "es"))
+            {
+                // Use the lower frame instead of the normal one
+                note_frame += "_low";
+            }
+        }
+
+
         // Get the cartesian point of the note
         geometry_msgs::PointStamped note_point;
         note_point.header.frame_id = note_frame;  // The frame of the note
@@ -207,11 +220,11 @@ std::vector<CartesianHitSequenceElement> hit_sequence_to_points(
             planning_frame,
             ros::Duration(1.0)
             );
-            
+
             // Insert the cartesian point and original message into the struct
             hit_sequence_element.left_mallet_point = note_point;
             hit_sequence_element.msg = point;
-            
+
             // Add the struct to the vector
             cartesian_hit_sequence.push_back(hit_sequence_element);
         }
@@ -221,15 +234,15 @@ std::vector<CartesianHitSequenceElement> hit_sequence_to_points(
             ROS_WARN("Skipping note %s", point.tone_name.c_str());
             continue;
         }
-        
+
     }
     return cartesian_hit_sequence;
 }
 
 
 /**
  * @brief Convert the timing information of a hit sequence from absolute to relative timings
- * 
+ *
  * @param hit_sequence
  * @return std::vector<CartesianHitSequenceElement>
  **/
@@ -266,15 +279,15 @@ std::vector<CartesianHitSequenceElement> hit_sequence_absolute_to_relative(
 
 /**
  * @brief Finds all chords in a sequence of notes and assigns the second malllet if one is detected
- * 
+ *
  * @param hits_relative Vector of notes with relative timing
  * @return std::vector<CartesianHitSequenceElement> Vector of notes with relative timing, both mallets are assigned if a chord is detected. This may be shorter than the input vector.
 */
 std::vector<CartesianHitSequenceElement> apply_chords(std::vector<CartesianHitSequenceElement> hits_relative)
 {
     // Create a copy of the input vector
     auto hits_relative_with_chords{hits_relative};
-    // Check if there are any hits with a (near) zero relative timing. 
+    // Check if there are any hits with a (near) zero relative timing.
     // These are chords and should be played at the same time using the second (right) mallet
     // Iterate over all hits
     for (auto hit_iter = hits_relative_with_chords.begin(); hit_iter != hits_relative_with_chords.end(); ++hit_iter) {