Cjans/wbc

README.md

@@ -10,6 +10,7 @@ This repository contains the `spot_manipulation_driver` package required to oper
   - Execute arm joint trajectories
   - Execute gripper trajectories
   - Execute arm and gripper trajectories as a single synced trajectory
+  - Execute body, arm and gripper trajectories as a single synced trajectory
   - Execute cartesian position/force trajectory for the EE
   - Given pixel coordinates, camera info, and a transform snapshot have the robot go and grasp the pixel-specified object in an image.
 
@@ -34,5 +35,9 @@ ros2 launch spot_moveit_config spot_execution.launch.py # same as above, but lau
 
 This package is configured to consider environmental obstacles localized to the map frame, so the `spot_navigation` localization capabilities need to be running for this to work. If you wish to operate without obstacle avoidance (which you shouldn't), you can comment out the `spot_mobility_joint` entry in the robot SRDF and comment out all of the 3D sensors in the `sensors_3d.yaml` file.
 
+## Spot_bringup Launch Args
+- `manipulation_action_namespace:="/spot_moveit"` for moveit-related planning
+- `kinematic_model:="mobile_manipulation"` to publish mobile-manipulation-related joint states
+
 ## Authors
 Janak Panthi (aka Crasun Jans) and Alex Navarro

spot_manipulation_driver/spot_manipulation_driver/spot_manipulation_driver.py

@@ -46,7 +46,7 @@
 from bosdyn.api.mobility_command_pb2 import MobilityCommand
 from bosdyn.api.arm_command_pb2 import ArmCommand, ArmJointMoveCommand, ArmJointPosition
 from bosdyn.client.frame_helpers import (ODOM_FRAME_NAME, GROUND_PLANE_FRAME_NAME, HAND_FRAME_NAME, BODY_FRAME_NAME,
-                                        GRAV_ALIGNED_BODY_FRAME_NAME, VISION_FRAME_NAME, get_a_tform_b)
+                                        GRAV_ALIGNED_BODY_FRAME_NAME, VISION_FRAME_NAME, get_a_tform_b, get_vision_tform_body)
 from bosdyn.client.math_helpers import SE3Pose
 from bosdyn.client.robot_command import (RobotCommandBuilder, blocking_command)
 from bosdyn.client.exceptions import RpcError
@@ -61,7 +61,19 @@
 
 MAX_BODY_POSES = 100
 MAX_ARM_POINTS = 10
+MAX_MOBILE_MANIPULATION_POINTS = 10
+ARM_NUM_JOINTS = 6
+GRIPPER_NUM_JOINTS = 1
+BODY_MOBILE_NUM_JOINTS = 3
+MOBILE_MANIPULATION_NUM_JOINTS = ARM_NUM_JOINTS + BODY_MOBILE_NUM_JOINTS + GRIPPER_NUM_JOINTS
 GRIPPER_CLOSE_TORQUE = 15.0
+MAX_GRIPPER_JOINT_VEL = 2.5  # rad/s
+MAX_GRIPPER_JOINT_ACC = 15.0  # rad/s^2
+MAX_ARM_JOINT_VEL = 2.5  # rad/s
+MAX_ARM_JOINT_ACC = 15.0  # rad/s^2
+MAX_BODY_VEL_LINEAR = 0.5  # m/s
+MAX_BODY_VEL_ANGULAR = 0.5  # rad/s
+BODY_HEIGHT_FOR_MOBILE_MANIPULATION = 0.0  # meters
 
 class GraspStrategy(Enum):
     TOP_DOWN_GRASP = 1
@@ -194,7 +206,7 @@ def verify_power_and_estop(self):
 
         self.verify_estop()
 
-    # Execute long trajectories
+    # Execute long arm trajectories
     def arm_long_trajectory_executor(
         self, traj_point_positions, traj_point_velocities, timepoints, cancel_event: threading.Event
     ) -> None:
@@ -464,6 +476,172 @@ def arm_and_gripper_long_trajectory_executor(
     #     self._logger.info("Successfully executed arm and gripper trajectory")
     #     return True
 
+    def create_mobile_manipulation_command(self, traj_points: list, times: list[float], ref_time: float, vision_T_body: SE3Pose) -> robot_command_pb2.RobotCommand:
+        """
+        Creates a synchronized command for mobile manipulation (i.e. body, arm, and gripper).
+
+        Args:
+            traj_point_positions: List of joint positions for each trajectory point in the mobile manipulation trajectory. Includes body and arm joints.
+            times: List of time offsets from reference time
+            ref_time: Reference timestamp for the trajectory
+            vision_T_body: Transform from body frame to vision frame
+
+        Returns:
+            RobotCommand with synchronized gripper, arm and mobility commands, or None on error
+        """
+
+        # Validate length of times and trajectory points
+        if len(times) != len(traj_points):
+            self._logger.error("Length mismatch between times and trajectory points.")
+            return None
+
+        # Validate number of joints in each trajectory point
+        for i, positions in enumerate(traj_points):
+            if len(positions) != MOBILE_MANIPULATION_NUM_JOINTS:
+                self._logger.error(f"Invalid number of joints at index {i}: expected {MOBILE_MANIPULATION_NUM_JOINTS}, got {len(positions)}")
+                return None
+
+        # Separate body and arm points
+        body_points = [point[:BODY_MOBILE_NUM_JOINTS] for point in traj_points]
+        arm_positions = [point[BODY_MOBILE_NUM_JOINTS:BODY_MOBILE_NUM_JOINTS + ARM_NUM_JOINTS] for point in traj_points]# Next ARM_NUM_JOINTS in each point are arm joints
+        gripper_positions_list = [point[-GRIPPER_NUM_JOINTS:] for point in traj_points] # Last GRIPPER_NUM_JOINTS in each point are gripper joints
+        gripper_positions = [elem for sublist in gripper_positions_list for elem in sublist] # flatten the list
+
+        # Build gripper command
+        gripper_command = RobotCommandBuilder.claw_gripper_command_helper(
+            gripper_positions=gripper_positions,
+            times=times,
+            ref_time=ref_time,
+            max_vel=MAX_GRIPPER_JOINT_VEL,
+            max_acc=MAX_GRIPPER_JOINT_ACC
+        )
+        # Build arm command
+        arm_command = RobotCommandBuilder.arm_joint_move_helper(
+            joint_positions=arm_positions,
+            times=times,
+            ref_time=ref_time,
+            max_vel=MAX_ARM_JOINT_VEL,
+            max_acc=MAX_ARM_JOINT_ACC
+        )
+
+        # Build se2 trajectory for the mobility command
+        # Transform body points to vision frame
+        se2_trajectory_points = []
+        for i in range(len(body_points)):
+            x_body = body_points[i][0]
+            y_body = body_points[i][1]
+            yaw_body = body_points[i][2]
+
+            # Transform to vision frame
+            x_vision, y_vision, _ = vision_T_body.transform_point(x_body, y_body, 0)
+            yaw_vision = vision_T_body.rot.to_yaw() + yaw_body
+
+            # Create SE2TrajectoryPoint
+            se2_pose = geometry_pb2.SE2Pose(
+                position=geometry_pb2.Vec2(x=x_vision, y=y_vision),
+                angle=yaw_vision
+            )
+            se2_trajectory_points.append((times[i], se2_pose))
+
+        # Build mobility params with velocity limits
+        speed_limit = geometry_pb2.SE2VelocityLimit(
+            max_vel=geometry_pb2.SE2Velocity(
+                linear=geometry_pb2.Vec2(x=MAX_BODY_VEL_LINEAR, y=MAX_BODY_VEL_LINEAR), 
+                angular=MAX_BODY_VEL_ANGULAR
+            ),
+            min_vel=geometry_pb2.SE2Velocity(
+                linear=geometry_pb2.Vec2(x=-MAX_BODY_VEL_LINEAR, y=-MAX_BODY_VEL_LINEAR), 
+                angular=-MAX_BODY_VEL_ANGULAR
+            )
+        )
+
+        mobility_params = spot_command_pb2.MobilityParams(vel_limit=speed_limit)
+
+        # Build the mobility command with mobility params and the final pose from the se2 trajectory
+        mobility_command = RobotCommandBuilder.synchro_se2_trajectory_command(
+            goal_se2=se2_trajectory_points[-1][1],  # Final pose
+            frame_name=VISION_FRAME_NAME,
+            params=mobility_params,
+            body_height=BODY_HEIGHT_FOR_MOBILE_MANIPULATION,
+            locomotion_hint=spot_command_pb2.HINT_AUTO
+        )
+
+        # Reset the se2 trajectory in the mobility command and manually add waypoints
+        se2_traj = mobility_command.synchronized_command.mobility_command.se2_trajectory_request.trajectory
+        del se2_traj.points[:]  # Clear existing points - protobuf repeated field
+
+        for time_offset, se2_pose in se2_trajectory_points:
+            point = se2_traj.points.add()
+            point.pose.CopyFrom(se2_pose)
+            point.time_since_reference.CopyFrom(seconds_to_duration(time_offset))
+
+        # Set reference time
+        if ref_time is not None:
+            mobility_command.synchronized_command.mobility_command.se2_trajectory_request.end_time.CopyFrom(ref_time)
+
+        # Combine gripper, arm and mobility commands into synchronized command
+        gripper_sync_command = robot_command_pb2.RobotCommand()
+        arm_sync_command = robot_command_pb2.RobotCommand()
+        mobility_sync_command = robot_command_pb2.RobotCommand()
+        gripper_sync_command.synchronized_command.gripper_command.CopyFrom(gripper_command.synchronized_command.gripper_command)
+        arm_sync_command.synchronized_command.arm_command.CopyFrom(arm_command.synchronized_command.arm_command)
+        mobility_sync_command.synchronized_command.mobility_command.CopyFrom(mobility_command.synchronized_command.mobility_command)
+        command = RobotCommandBuilder.build_synchro_command(mobility_sync_command, arm_sync_command, gripper_sync_command)
+
+        return command
+
+    # Execute long mobile manipulation trajectories
+    def mobile_manipulation_long_trajectory_executor(
+        self, traj_point_positions, traj_point_velocities, timepoints, cancel_event: threading.Event
+    ) -> None:
+
+        # Make sure the robot is powered on (which implicitly implies that it's estopped as well)
+        try:
+            if self._lease_manager.robot.is_powered_on(5):
+                self._logger.info("Spot is about to move its whole body.")
+            else:
+                self._logger.warn("Cannot execute mobile manipulation long trajectory, robot is not powered on")
+                return False
+        except RpcError as e:
+            self._logger.warn(f"Cannot execute mobile manipulation long trajectory, unable to communicate with robot\n\tMsg: {e}")
+            return False
+
+        start_time = time.time()
+        ref_time = seconds_to_timestamp(start_time)
+
+        # Get the latest robot state for transforms
+        robot_state = self._lease_manager._robot_state_client.get_robot_state()
+        vision_T_body = get_vision_tform_body(robot_state.kinematic_state.transforms_snapshot)
+
+        # Initialize trajectory manager to extract slices from the original trajectory for execution
+        mobile_manipulation_trajectory_manager = TrajectoryManager(
+            points=traj_point_positions,
+            times_since_ref=timepoints,
+            ref_time=start_time
+        )
+
+        while not mobile_manipulation_trajectory_manager.done():
+            if cancel_event.is_set():
+                self.stop_robot()
+                self._logger.info("Mobile manipulation trajectory action cancelled early. Stopping robot")
+                return False
+
+            window, timestamps, sleep_time = mobile_manipulation_trajectory_manager.get_window(MAX_MOBILE_MANIPULATION_POINTS)
+
+            robot_cmd = self.create_mobile_manipulation_command(window, timestamps, ref_time, vision_T_body)
+
+            success, msg, _ = self._lease_manager.robot_command(robot_cmd, end_time_secs=time.time()+sleep_time)
+
+            if not success:
+                self._logger.warn(f"mobile_manipulation_long_trajectory_executor: Error executing robot command: {msg}")
+                return False
+
+            if mobile_manipulation_trajectory_manager.last():
+                time.sleep(sleep_time)
+            else:
+                time.sleep(max(sleep_time - 0.2, 0))
+        self._logger.info("Successfully executed mobile manipulation trajectory")
+        return True
 
     def body_manipulation_trajectory_executor(
             self, body_trajectory: List[SE3Pose], arm_trajectory: List[List[float]], timestamps: List[float]

spot_manipulation_driver/spot_manipulation_driver/spot_manipulation_driver_ros.py

@@ -80,6 +80,11 @@
 GRIPPER_JOINT_ORDER = [
     "arm0_fingers"]
 
+WHOLE_BODY_JOINT_ORDER = [
+    "body_x",
+    "body_y",
+    "body_or"] + ARM_JOINT_ORDER + GRIPPER_JOINT_ORDER
+
 
 class SpotManipulationDriverROS(Node):
     def __init__(self):
@@ -100,6 +105,7 @@ def __init__(self):
 
         self._arm_trajectory_cancel_event = threading.Event()
         self._arm_and_finger_trajectory_cancel_event = threading.Event()
+        self._mobile_manipulation_trajectory_cancel_event = threading.Event()
         self._arm_cartesian_command_cancel_event = threading.Event()
 
         # Declare ROS parameters
@@ -170,6 +176,9 @@ def __init__(self):
         # Arm-and-finger-related attributes
         self.arm_and_finger_result = FollowJointTrajectory.Result()
 
+        # Mobile-manipulation-related attributes
+        self.mobile_manipulation_result = FollowJointTrajectory.Result()
+
         # Image_to_grasp-related attributes
         self.image_to_grasp_result = ImageToGrasp.Result()
 
@@ -206,6 +215,7 @@ def connect(self, lease_manager: SpotLeaseManager) -> bool:
 
         self._joint_state_pub = self.create_publisher(JointState, "/joint_states", 10)
         self._arm_goal_pub = self.create_publisher(JointTrajectory, "/arm_controller/follow_joint_trajectory/goal", 10)
+        self._mobile_manipulation_goal_pub = self.create_publisher(JointTrajectory, "/mobile_manipulation_controller/follow_joint_trajectory/goal", 10)
 
         # Command subscriptions
         self.ee_vel_sub = self.create_subscription(
@@ -267,6 +277,15 @@ def connect(self, lease_manager: SpotLeaseManager) -> bool:
             cancel_callback=self.arm_and_finger_goal_cancel_callback
         )
 
+        self.mobile_manipulation_action_server = ActionServer(
+            self,
+            FollowJointTrajectory,
+            f"{action_ns}/mobile_manipulation_controller/follow_joint_trajectory",
+            self.mobile_manipulation_goal_callback,
+            callback_group=motion_callback_group,
+            cancel_callback=self.mobile_manipulation_goal_cancel_callback
+        )
+
         self.body_manipulation_action_server = ActionServer(
             self,
             FollowJointTrajectory,
@@ -491,6 +510,72 @@ def body_manipulation_callback(self, goal_handle: ServerGoalHandle) -> FollowJoi
             error_string = "Exception occured"
 
         return FollowJointTrajectory.Result(error_code=error_code, error_string=error_string)
+
+    def mobile_manipulation_goal_cancel_callback(self, cancel_request):
+        self._mobile_manipulation_trajectory_cancel_event.set()
+        return CancelResponse.ACCEPT
+
+    def mobile_manipulation_goal_callback(self, goal_handle: ServerGoalHandle):
+        """Callback for the /mobile_manipulation_controller/follow_joint_trajectory action server """
+
+        # Translate message and execute trajectory while publishing feedback
+        traj_point_positions, traj_point_velocities, timepoints = ros_helpers.joint_trajectory_to_lists(
+            goal_handle.request.trajectory, WHOLE_BODY_JOINT_ORDER
+        )
+
+        # Publish the received trajectory (only once) before executing for data-capture purposes
+        if self.data_capture_mode:
+            def mobile_manipulation_goal_publisher() -> None:
+                timeout = 3.0
+                start_time = time.time()  
+
+                while self._mobile_manipulation_goal_pub.get_subscription_count() < 1:
+                    if time.time() - start_time > timeout:
+                        self.get_logger().info("Timed out waiting for an mobile_manipulation_controller/follow_joint_trajectory/goal subscriber to be available")
+                        return  
+                    time.sleep(0.1)  # sleep to avoid busy-waiting
+                self.get_logger().info("Publishing received joint trajectory on the mobile_manipulation_controller/follow_joint_trajectory/goal topic")
+                self._mobile_manipulation_goal_pub.publish(goal_handle.request.trajectory)
+
+            mobile_manipulation_goal_publisher_thread = threading.Thread(target=mobile_manipulation_goal_publisher)
+            mobile_manipulation_goal_publisher_thread.start()
+
+        trajectory_success = False
+
+        def execute_trajectory() -> None:
+            nonlocal trajectory_success
+            try:
+                trajectory_success = self.manipulation_driver.mobile_manipulation_long_trajectory_executor(
+                    traj_point_positions, traj_point_velocities, timepoints, self._mobile_manipulation_trajectory_cancel_event
+                ) 
+            except Exception as e:
+                self._logger.info(f"Error executing mobile manipulation long trajectory: {e}")
+                return
+
+        mobile_manipulation_execution_thread = threading.Thread(target=execute_trajectory)
+        mobile_manipulation_execution_thread.start()
+
+        rate = self.create_rate(10.0)
+        while mobile_manipulation_execution_thread.is_alive():
+            goal_handle.publish_feedback(
+                ros_helpers.get_joint_state_feedback(self.manipulation_driver)
+            )
+            rate.sleep()
+
+        # Join threads
+        mobile_manipulation_execution_thread.join()
+        if self.data_capture_mode:
+            mobile_manipulation_goal_publisher_thread.join()
+
+        if self._mobile_manipulation_trajectory_cancel_event.is_set():
+            goal_handle.canceled()
+            self._mobile_manipulation_trajectory_cancel_event.clear()
+        elif trajectory_success:
+            goal_handle.succeed()
+        else:
+            goal_handle.abort()
+
+        return self.mobile_manipulation_result
 
     def arm_cartesian_command_cancel_callback(self, cancel_request):
         self._arm_cartesian_command_cancel_event.set()

spot_moveit_config/config/controllers.yaml

@@ -29,3 +29,27 @@ spot_moveit/arm_controller:
       goal_time: 0.0
     update_rate:
       30
+
+spot_moveit/mobile_manipulation_controller:
+  ros__parameters:
+    joints:
+      - body_x
+      - body_y
+      - body_or
+      - arm0_shoulder_yaw
+      - arm0_shoulder_pitch
+      - arm0_elbow_pitch
+      - arm0_elbow_roll
+      - arm0_wrist_pitch
+      - arm0_wrist_roll
+      - arm0_fingers
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    allow_partial_joints_goal: false
+    constraints:
+      goal_time: 0.0
+    update_rate:
+      30