Cleaning up errors from previous multi-task version

Author: Federico-PizarroBejarano

mm_control/scripts/generate_acados_code.py

@@ -1,6 +1,6 @@
 import argparse
 
-import mm_control.MPC as MPC
+from mm_control.MPC import MPC
 from mm_utils import parsing
 
 if __name__ == "__main__":
@@ -24,7 +24,11 @@
     config["controller"]["acados"]["cython"]["recompile"] = True
 
     ctrl_config = config["controller"]
-    control_class = getattr(MPC, ctrl_config["type"], None)
-    if control_class is None:
-        raise ValueError(f"Unknown controller type: {ctrl_config['type']}")
+    ctrl_type = ctrl_config["type"]
+
+    if ctrl_type == "MPC":
+        control_class = MPC
+    else:
+        raise ValueError(f"Unknown controller type: {ctrl_type}")
+
     controller = control_class(ctrl_config)

mm_control/src/mm_control/MPCCostFunctions.py

@@ -506,10 +506,16 @@ def create_regularization_cost(robot_mdl, params):
     return RegularizationCostFunction(ss_mdl["nx"], ss_mdl["nu"])
 
 
+def create_manipulability_cost(robot_mdl, params):
+    """Create manipulability cost"""
+    return ManipulabilityCostFunction(robot_mdl)
+
+
 # Register core cost functions (simplified to 6 parameterized functions)
 CostFunctionRegistry.register("BasePose", create_base_pose_cost)
 CostFunctionRegistry.register("BaseVel", create_base_vel_cost)
 CostFunctionRegistry.register("EEPose", create_ee_pose_cost)
 CostFunctionRegistry.register("EEVel", create_ee_vel_cost)
 CostFunctionRegistry.register("ControlEffort", ControlEffortCostFunction)
 CostFunctionRegistry.register("Regularization", create_regularization_cost)
+CostFunctionRegistry.register("Manipulability", create_manipulability_cost)

mm_plan/src/mm_plan/TaskManager.py

@@ -25,7 +25,7 @@ def activatePlanners(self):
             planner.activate()
 
     def getPlanner(self):
-        """Get the active planners (current task and next if available).
+        """Get the active planner (current task).
 
         Returns:
             Active planner

mm_run/config/test_experiments/test_position_only.yaml

@@ -7,8 +7,8 @@ planner:
   tasks:
   - name: EE Position Only
     planner_type: WaypointPlanner
-    base_pose: [2, 0.4, 0]   # x, y, yaw in world frame
-    ee_pose: [2.5, 0.4, 1.0, 0, 0, 0]  # World frame: [x, y, z, roll, pitch, yaw]
+    base_pose: [2, 0.4, 0.5]   # x, y, yaw in world frame
+    ee_pose: [2.5, 0.4, 1.0, 0.5, 0, 0]  # World frame: [x, y, z, roll, pitch, yaw]
     tracking_err_tol: 0.05
 
 controller:

mm_run/nodes/mpc_ros.py

@@ -215,61 +215,60 @@ def _publish_cmd_vel_new(self, event):
 
         self.robot_interface.publish_cmd_vel(self.cmd_vel)
 
-    def _publish_trajectory_tracking_pt(self, t, robot_states, planners):
+    def _publish_trajectory_tracking_pt(self, t, robot_states, planner):
         msg = MultiDOFJointTrajectory()
         msg.header.stamp = rospy.Time.now()
 
-        for planner in planners:
-            # Get base reference if available
-            if planner.has_base_ref:
-                p, v = planner.getBaseTrackingPoint(t, robot_states)
-                if p is not None:
-                    msg.joint_names.append("base")
-                    pt_msg = MultiDOFJointTrajectoryPoint()
-                    transform = Transform()
-                    transform.translation.x = p[0]
-                    transform.translation.y = p[1]
-                    transform.translation.z = 0.25  # Display height
-                    quat = tf.quaternion_from_euler(0, 0, p[2])
-                    transform.rotation.x = quat[0]
-                    transform.rotation.y = quat[1]
-                    transform.rotation.z = quat[2]
-                    transform.rotation.w = quat[3]
-                    pt_msg.transforms.append(transform)
-                    if v is not None:
-                        velocity = Twist()
-                        velocity.linear.x = v[0]
-                        velocity.linear.y = v[1]
-                        velocity.angular.z = v[2]
-                        pt_msg.velocities.append(velocity)
-                    msg.points.append(pt_msg)
-
-            # Get EE reference if available
-            if planner.has_ee_ref:
-                p, v = planner.getEETrackingPoint(t, robot_states)
-                if p is not None:
-                    msg.joint_names.append("EE")
-                    pt_msg = MultiDOFJointTrajectoryPoint()
-                    transform = Transform()
-                    transform.translation.x = p[0]
-                    transform.translation.y = p[1]
-                    transform.translation.z = p[2]
-                    quat = tf.quaternion_from_euler(*p[3:])
-                    transform.rotation.x = quat[0]
-                    transform.rotation.y = quat[1]
-                    transform.rotation.z = quat[2]
-                    transform.rotation.w = quat[3]
-                    pt_msg.transforms.append(transform)
-                    if v is not None:
-                        velocity = Twist()
-                        velocity.linear.x = v[0]
-                        velocity.linear.y = v[1]
-                        velocity.linear.z = v[2]
-                        velocity.angular.x = v[3] if len(v) > 3 else 0
-                        velocity.angular.y = v[4] if len(v) > 4 else 0
-                        velocity.angular.z = v[5] if len(v) > 5 else 0
-                        pt_msg.velocities.append(velocity)
-                    msg.points.append(pt_msg)
+        # Get base reference if available
+        if planner.has_base_ref:
+            p, v = planner.getBaseTrackingPoint(t, robot_states)
+            if p is not None:
+                msg.joint_names.append("base")
+                pt_msg = MultiDOFJointTrajectoryPoint()
+                transform = Transform()
+                transform.translation.x = p[0]
+                transform.translation.y = p[1]
+                transform.translation.z = 0.25  # Display height
+                quat = tf.quaternion_from_euler(0, 0, p[2])
+                transform.rotation.x = quat[0]
+                transform.rotation.y = quat[1]
+                transform.rotation.z = quat[2]
+                transform.rotation.w = quat[3]
+                pt_msg.transforms.append(transform)
+                if v is not None:
+                    velocity = Twist()
+                    velocity.linear.x = v[0]
+                    velocity.linear.y = v[1]
+                    velocity.angular.z = v[2]
+                    pt_msg.velocities.append(velocity)
+                msg.points.append(pt_msg)
+
+        # Get EE reference if available
+        if planner.has_ee_ref:
+            p, v = planner.getEETrackingPoint(t, robot_states)
+            if p is not None:
+                msg.joint_names.append("EE")
+                pt_msg = MultiDOFJointTrajectoryPoint()
+                transform = Transform()
+                transform.translation.x = p[0]
+                transform.translation.y = p[1]
+                transform.translation.z = p[2]
+                quat = tf.quaternion_from_euler(*p[3:])
+                transform.rotation.x = quat[0]
+                transform.rotation.y = quat[1]
+                transform.rotation.z = quat[2]
+                transform.rotation.w = quat[3]
+                pt_msg.transforms.append(transform)
+                if v is not None:
+                    velocity = Twist()
+                    velocity.linear.x = v[0]
+                    velocity.linear.y = v[1]
+                    velocity.linear.z = v[2]
+                    velocity.angular.x = v[3] if len(v) > 3 else 0
+                    velocity.angular.y = v[4] if len(v) > 4 else 0
+                    velocity.angular.z = v[5] if len(v) > 5 else 0
+                    pt_msg.velocities.append(velocity)
+                msg.points.append(pt_msg)
 
         if len(msg.points) > 0:
             self.tracking_point_pub.publish(msg)
@@ -361,51 +360,49 @@ def _publish_planner_data(self, event):
                     marker_plan.lifetime = rospy.Duration.from_sec(0.1)
                     self.plan_visualization_pub.publish(marker_plan)
 
-        curr_planners = self.sot.getPlanners(2)
-        colors = [[1, 0, 0], [0, 1, 0]]
-        for pid, planner in enumerate(curr_planners):
-            # Visualize current base waypoint/path if available
-            if planner.has_base_ref:
-                if planner.ref_type == RefType.WAYPOINT:
-                    quat = tf.quaternion_from_euler(0, 0, planner.base_target[2])
-                    pose_msg = PoseStamped()
-                    pose_msg.header.stamp = rospy.Time()
-                    pose_msg.pose.position = Point(*planner.base_target[:2], 0.25)
-                    pose_msg.pose.orientation = Quaternion(*list(quat))
-                    self.pose_plan_visualization_pub.publish(pose_msg)
-                elif planner.ref_type == RefType.PATH:
-                    marker_plan = self._make_marker(
-                        Marker.POINTS,
-                        pid,
-                        rgba=colors[pid] + [1],
-                        scale=[0.1, 0.1, 0.1],
-                    )
-                    marker_plan.points = [
-                        Point(*pt[:2], 0) for pt in planner.base_plan["p"]
-                    ]
-                    marker_plan.lifetime = rospy.Duration.from_sec(0.1)
-                    self.current_plan_visualization_pub.publish(marker_plan)
-
-            # Visualize current EE waypoint/path if available
-            if planner.has_ee_ref:
-                if planner.ref_type == RefType.WAYPOINT:
-                    quat = tf.quaternion_from_euler(*planner.ee_target[3:])
-                    pose_msg = PoseStamped()
-                    pose_msg.header.frame_id = "world"
-                    pose_msg.header.stamp = rospy.Time()
-                    pose_msg.pose.position = Point(*planner.ee_target[:3])
-                    pose_msg.pose.orientation = Quaternion(*list(quat))
-                    self.pose_plan_visualization_pub.publish(pose_msg)
-                elif planner.ref_type == RefType.PATH:
-                    marker_plan = self._make_marker(
-                        Marker.POINTS,
-                        pid + 100,
-                        rgba=colors[pid] + [1],
-                        scale=[0.1, 0.1, 0.1],
-                    )
-                    marker_plan.points = [Point(*pt[:3]) for pt in planner.ee_plan["p"]]
-                    marker_plan.lifetime = rospy.Duration.from_sec(0.1)
-                    self.current_plan_visualization_pub.publish(marker_plan)
+        planner = self.sot.getPlanner()
+        # Visualize current base waypoint/path if available
+        if planner.has_base_ref:
+            if planner.ref_type == RefType.WAYPOINT:
+                quat = tf.quaternion_from_euler(0, 0, planner.base_target[2])
+                pose_msg = PoseStamped()
+                pose_msg.header.stamp = rospy.Time()
+                pose_msg.pose.position = Point(*planner.base_target[:2], 0.25)
+                pose_msg.pose.orientation = Quaternion(*list(quat))
+                self.pose_plan_visualization_pub.publish(pose_msg)
+            elif planner.ref_type == RefType.PATH:
+                marker_plan = self._make_marker(
+                    Marker.POINTS,
+                    0,
+                    rgba=[1, 0, 0, 1],
+                    scale=[0.1, 0.1, 0.1],
+                )
+                marker_plan.points = [
+                    Point(*pt[:2], 0) for pt in planner.base_plan["p"]
+                ]
+                marker_plan.lifetime = rospy.Duration.from_sec(0.1)
+                self.current_plan_visualization_pub.publish(marker_plan)
+
+        # Visualize current EE waypoint/path if available
+        if planner.has_ee_ref:
+            if planner.ref_type == RefType.WAYPOINT:
+                quat = tf.quaternion_from_euler(*planner.ee_target[3:])
+                pose_msg = PoseStamped()
+                pose_msg.header.frame_id = "world"
+                pose_msg.header.stamp = rospy.Time()
+                pose_msg.pose.position = Point(*planner.ee_target[:3])
+                pose_msg.pose.orientation = Quaternion(*list(quat))
+                self.pose_plan_visualization_pub.publish(pose_msg)
+            elif planner.ref_type == RefType.PATH:
+                marker_plan = self._make_marker(
+                    Marker.POINTS,
+                    100,
+                    rgba=[0, 1, 0, 1],
+                    scale=[0.1, 0.1, 0.1],
+                )
+                marker_plan.points = [Point(*pt[:3]) for pt in planner.ee_plan["p"]]
+                marker_plan.lifetime = rospy.Duration.from_sec(0.1)
+                self.current_plan_visualization_pub.publish(marker_plan)
 
         self.sot_lock.release()
 
@@ -520,9 +517,6 @@ def run(self):
         t0 = t
         self.sot.started = True
 
-        sot_num_plans = self.ctrl_config.get("task_num", None)
-        if sot_num_plans is None:
-            sot_num_plans = 1 if self.ctrl_config["type"][:2] == "ST" else 2
         while not self.ctrl_c:
             t = rospy.Time.now().to_sec()
 
@@ -558,10 +552,8 @@ def run(self):
             # if the robot is very close to the goal, stop the robot
             # Check if any active planner is close to finish
             self.sot_lock.acquire()
-            active_planners = self.sot.getPlanners(num_planners=sot_num_plans)
-            close_to_goal = False
-            for planner in active_planners:
-                close_to_goal = close_to_goal or planner.closeToFinish()
+            planner = self.sot.getPlanner()
+            close_to_goal = planner.closeToFinish()
             self.sot_lock.release()
             if close_to_goal:
                 print("Close to goal. Braking")
@@ -597,11 +589,11 @@ def run(self):
 
             # publish data
             self._publish_mpc_data(self.controller)
-            # Get active planners for visualization
+            # Get active planner for visualization
             self.sot_lock.acquire()
-            active_planners = self.sot.getPlanners(num_planners=sot_num_plans)
+            active_planner = self.sot.getPlanner()
             self.sot_lock.release()
-            self._publish_trajectory_tracking_pt(t - t0, robot_states, active_planners)
+            self._publish_trajectory_tracking_pt(t - t0, robot_states, active_planner)
 
             # Update Task Manager
             # Convert to pose arrays in world frame

mm_run/nodes/planner_test_ros.py

@@ -157,42 +157,6 @@ def _publish_planner_data(self, event):
             marker_plan.lifetime = rospy.Duration.from_sec(0.1)
             self.plan_visualization_pub.publish(marker_plan)
 
-        curr_planners = self.sot.getPlanners(2)
-        colors = [[1, 0, 0], [0, 1, 0]]
-        for pid, planner in enumerate(curr_planners):
-            if planner.ref_type == "waypoint":
-                if planner.ref_data_type == "Vec3":
-                    marker_plan = self._make_marker(
-                        Marker.SPHERE,
-                        pid,
-                        rgba=colors[pid] + [1],
-                        scale=[0.1, 0.1, 0.1],
-                    )
-                    marker_plan.pose.position = Point(*planner.target_pos)
-                elif planner.ref_data_type == "Vec2":
-                    marker_plan = self._make_marker(
-                        Marker.CYLINDER,
-                        pid,
-                        rgba=colors[pid] + [1],
-                        scale=[0.1, 0.1, 0.5],
-                    )
-                    marker_plan.pose.position = Point(*planner.target_pos, 0.25)
-            elif planner.ref_type == "trajectory":
-                marker_plan = self._make_marker(
-                    Marker.LINE_STRIP,
-                    pid,
-                    rgba=colors[pid] + [1],
-                    scale=[0.1, 0.1, 0.1],
-                )
-
-                if planner.ref_data_type == "Vec3":
-                    marker_plan.points = [Point(*pt) for pt in planner.plan["p"]]
-                elif planner.ref_data_type == "Vec2":
-                    marker_plan.points = [Point(*pt, 0) for pt in planner.plan["p"]]
-
-            marker_plan.lifetime = rospy.Duration.from_sec(0.1)
-            self.current_plan_visualization_pub.publish(marker_plan)
-
         self.sot_lock.release()
 
     def run(self):
@@ -215,22 +179,19 @@ def run(self):
         rospy.Time.now().to_sec()
         self.sot.started = True
 
-        sot_num_plans = self.ctrl_config.get("task_num", None)
-        if sot_num_plans is None:
-            sot_num_plans = 1 if self.ctrl_config["type"][:2] == "ST" else 2
         while not self.ctrl_c:
             rospy.Time.now().to_sec()
 
             # open-loop command
             robot_states = (self.robot_interface.q, self.robot_interface.v)
-            planners = self.sot.getPlanners(num_planners=sot_num_plans)
+            planner = self.sot.getPlanner()
 
-            planners[0].updateRobotStates(robot_states)  # need robot pose
+            planner.updateRobotStates(robot_states)  # need robot pose
 
-            if planners[0].plan is not None:
-                print("plan t\n", planners[0].plan["t"].shape, planners[0].plan["t"])
-                print("plan p\n", planners[0].plan["p"].shape, planners[0].plan["p"])
-                print("plan v\n", planners[0].plan["v"].shape, planners[0].plan["v"])
+            if planner.plan is not None:
+                print("plan t\n", planner.plan["t"].shape, planner.plan["t"])
+                print("plan p\n", planner.plan["p"].shape, planner.plan["p"])
+                print("plan v\n", planner.plan["v"].shape, planner.plan["v"])
             else:
                 print("no plan")