Cjans/wbc #1
Cjans/wbc #1
Conversation
alexnavtt
left a comment
alexnavtt
left a comment
There was a problem hiding this comment.
Looks pretty clean overall, just some minor changes. One thing that I would like you to double check before we merge this is that moveit behaves properly (no warnings, errors, etc.) if you launch the moveit server with the kinematic model set to something other than mobile_manipulation.
| 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 |
There was a problem hiding this comment.
Are these hardware limitations or "good values" based on testing? If it's the latter, then we'd probably want to make them parameters instead of hard-coding
| # 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: |
There was a problem hiding this comment.
Can you add the list element type for traj_points?
| # Build se2 trajectory for the mobility command | ||
| # Transform body points to vision frame | ||
| se2_trajectory_points = [] | ||
| for i in range(len(body_points)): |
There was a problem hiding this comment.
This would be cleaner as an enumerated for loop
| if ref_time is not None: | ||
| mobility_command.synchronized_command.mobility_command.se2_trajectory_request.end_time.CopyFrom(ref_time) |
There was a problem hiding this comment.
Just to double check, if ref_time actually is None is the behavior of the function still well defined?
Enables mobile manipulation by implementing the ROS action server
/mobile_manipulation_controller/follow_joint_trajectoryalong with relevant Spot wrapper and helper functions to execute whole-body trajectories for Spot, including coordinated body, arm, and gripper control.