wip MoveIt Planning Features

docs/examples/03_backends_ros/files/04_plan_motion.py

@@ -5,9 +5,11 @@
 from compas_fab.backends import RosClient
 from compas_fab.robots import FrameTarget
 from compas_fab.robots import TargetMode
+from compas_fab.backends import MoveItPlanner
 
 with RosClient() as client:
     robot = client.load_robot()
+    planner = MoveItPlanner(client)
     assert robot.name == "ur5_robot"
 
     frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
@@ -26,7 +28,12 @@
         tolerance_orientation,
     )
 
-    trajectory = robot.plan_motion(target, start_configuration, group, options=dict(planner_id="RRTConnect"))
+    options = {
+        "planner_id": "RRTConnect",
+        # "max_velocity_scaling_factor": 0.01,
+        # "max_acceleration_scaling_factor": 0.01,
+    }
+    trajectory = planner.plan_motion(target, start_configuration, group, options=options)
 
     print("Computed kinematic path with %d configurations." % len(trajectory.points))
     print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)

src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py

@@ -14,6 +14,19 @@
 from compas_fab.backends.ros.messages import RobotState
 from compas_fab.backends.ros.service_description import ServiceDescription
 
+from compas import IPY
+
+if not IPY:
+    from typing import TYPE_CHECKING
+
+    if TYPE_CHECKING:
+        from compas_fab.backends.ros.client import RosClient  # noqa: F401
+        from compas_fab.backends.ros.planner import MoveItPlanner  # noqa: F401
+        from compas_fab.robots import Robot  # noqa: F401
+        from compas_fab.robots import TargetMode  # noqa: F401
+        from compas_fab.robots import RobotCellState  # noqa: F401
+        from compas.geometry import Frame  # noqa: F401
+
 __all__ = [
     "MoveItForwardKinematics",
 ]
@@ -26,17 +39,16 @@ class MoveItForwardKinematics(ForwardKinematics):
         "/compute_fk", "GetPositionFK", GetPositionFKRequest, GetPositionFKResponse, validate_response
     )
 
-    def forward_kinematics(self, robot, configuration, group=None, options=None):
+    def forward_kinematics(self, robot_cell_state, target_mode, group=None, options=None):
+        # type: (RobotCellState, TargetMode, str, dict) -> Frame
         """Calculate the robot's forward kinematic.
 
         Parameters
         ----------
-        robot : :class:`compas_fab.robots.Robot`
-            The robot instance for which inverse kinematics is being calculated.
-        configuration : :class:`compas_fab.robots.Configuration`
-            The full configuration to calculate the forward kinematic for. If no
-            full configuration is passed, the zero-joint state for the other
-            configurable joints is assumed.
+        robot_cell_state : :class:`compas_fab.robots.RobotCellState`
+            The robot cell state.
+        target_mode : :class:`compas_fab.robots.TargetMode`
+            The target mode.
         group : str, optional
             Unused parameter.
         options : dict, optional
@@ -56,21 +68,30 @@ def forward_kinematics(self, robot, configuration, group=None, options=None):
         :class:`Frame`
             The frame in the world's coordinate system (WCF).
         """
+        planner = self  # type: MoveItPlanner
+        client = planner.client  # type: RosClient
+        robot = client.robot  # type: Robot
         options = options or {}
+
+        planner.set_robot_cell_state(robot_cell_state)
+
         kwargs = {}
-        kwargs["configuration"] = configuration
+        kwargs["configuration"] = robot_cell_state.robot_configuration
         kwargs["options"] = options
         kwargs["errback_name"] = "errback"
 
         # Use base_link or fallback to model's root link
         options["base_link"] = options.get("base_link", robot.model.root.name)
 
+        group = group or robot.main_group_name
+
         # Use selected link or default to group's end effector
         options["link"] = options.get("link", options.get("tool0")) or robot.get_end_effector_link_name(group)
         if options["link"] not in robot.get_link_names(group):
             raise ValueError("Link name {} does not exist in planning group".format(options["link"]))
 
-        return await_callback(self.forward_kinematics_async, **kwargs)
+        pcf_frame = await_callback(self.forward_kinematics_async, **kwargs)
+        return client.robot_cell.pcf_to_target_frames(robot_cell_state, pcf_frame, target_mode, group)
 
     def forward_kinematics_async(self, callback, errback, configuration, options):
         """Asynchronous handler of MoveIt FK service."""

src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py

@@ -144,14 +144,15 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options=
 
         """
         options = options or {}
-        robot = self.client.robot
-
+        planner = self  # type: MoveItPlanner
+        robot = planner.client.robot
+        client = planner.client
         # Group
         group = group or robot.main_group_name
 
         # Set scene
         # TODO: Implement start_state
-        self.robot_cell.assert_cell_state_match(start_state)
+        client.robot_cell.assert_cell_state_match(start_state)
         start_state = start_state or RobotCellState.from_robot_configuration(robot)
 
         # Start configuration
@@ -164,12 +165,12 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options=
         target_frame = target.target_frame
 
         # Attached tool frames does not need scaling because Tools are modelled in meter scale
-        attached_tool = self.robot_cell.get_attached_tool(start_state, group)
+        attached_tool = client.robot_cell.get_attached_tool(start_state, group)
         if attached_tool:
             target_frame = from_tcf_to_t0cf(target_frame, attached_tool.frame)
 
         # Scale Attached Collision Meshes
-        attached_collision_meshes = self.robot_cell.get_attached_rigid_bodies_as_attached_collision_meshes(
+        attached_collision_meshes = client.robot_cell.get_attached_rigid_bodies_as_attached_collision_meshes(
             start_state, group
         )
         # TODO: ACM will always be modeled in meters too. This is a temporary solution

src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py

@@ -2,25 +2,36 @@
 from __future__ import division
 from __future__ import print_function
 
+from compas import IPY
 from compas.utilities import await_callback
 
 from compas_fab.backends.interfaces import PlanCartesianMotion
 from compas_fab.backends.ros.backend_features.helpers import convert_constraints_to_rosmsg
 from compas_fab.backends.ros.backend_features.helpers import convert_trajectory
 from compas_fab.backends.ros.backend_features.helpers import validate_response
-from compas_fab.backends.ros.messages import AttachedCollisionObject
 from compas_fab.backends.ros.messages import GetCartesianPathRequest
 from compas_fab.backends.ros.messages import GetCartesianPathResponse
 from compas_fab.backends.ros.messages import Header
-from compas_fab.backends.ros.messages import JointState
-from compas_fab.backends.ros.messages import MultiDOFJointState
 from compas_fab.backends.ros.messages import Pose
-from compas_fab.backends.ros.messages import RobotState
 from compas_fab.backends.ros.service_description import ServiceDescription
-
 from compas_fab.robots import FrameWaypoints
 from compas_fab.robots import PointAxisWaypoints
 
+if not IPY:
+    from typing import TYPE_CHECKING
+
+    if TYPE_CHECKING:
+        from typing import Callable  # noqa: F401
+        from typing import Dict  # noqa: F401
+        from typing import Optional  # noqa: F401
+
+        from compas_fab.backends import MoveItPlanner  # noqa: F401
+        from compas_fab.backends import RosClient  # noqa: F401
+        from compas_fab.backends.ros.messages import PlanningScene  # noqa: F401
+        from compas_fab.robots import JointTrajectory  # noqa: F401
+        from compas_fab.robots import RobotCellState  # noqa: F401
+        from compas_fab.robots import Waypoints  # noqa: F401
+
 __all__ = [
     "MoveItPlanCartesianMotion",
 ]
@@ -37,7 +48,8 @@ class MoveItPlanCartesianMotion(PlanCartesianMotion):
         validate_response,
     )
 
-    def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, options=None):
+    def plan_cartesian_motion(self, waypoints, start_state, group=None, options=None):
+        # type: (Waypoints, RobotCellState, Optional[str], Optional[Dict]) -> JointTrajectory
         """Calculates a cartesian motion path (linear in tool space).
 
         Parameters
@@ -46,10 +58,9 @@ def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None,
             The robot instance for which the cartesian motion plan is being calculated.
         waypoints : :class:`compas_fab.robots.Waypoints`
             The waypoints for the robot to follow.
-        start_configuration: :class:`compas_robots.Configuration`, optional
-            The robot's full configuration, i.e. values for all configurable
-            joints of the entire robot, at the starting position. Defaults to
-            the all-zero configuration.
+        start_state : :class:`compas_fab.robots.RobotCellState`
+            The starting state of the robot cell at the beginning of the motion.
+            The attribute `robot_configuration`, must be provided.
         group: str, optional
             The planning group used for calculation. Defaults to the robot's
             main planning group.
@@ -82,14 +93,17 @@ def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None,
         :class:`compas_fab.robots.JointTrajectory`
             The calculated trajectory.
         """
-        robot = self.client.robot
+        planner = self  # type: MoveItPlanner  # noqa: F841
+        client = planner.client  # type: RosClient # noqa: F841
+        robot = client.robot
 
         options = options or {}
         kwargs = {}
         kwargs["options"] = options
         kwargs["waypoints"] = waypoints
-        kwargs["start_configuration"] = start_configuration
-        kwargs["group"] = group or robot.main_group_name
+        kwargs["start_state"] = start_state
+        group = group or robot.main_group_name
+        kwargs["group"] = group
 
         kwargs["errback_name"] = "errback"
 
@@ -110,33 +124,41 @@ def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None,
             raise TypeError("Unsupported waypoints type {} for MoveIt planning backend.".format(type(waypoints)))
 
     def plan_cartesian_motion_with_frame_waypoints_async(
-        self, callback, errback, waypoints, start_configuration=None, group=None, options=None
+        self, callback, errback, waypoints, start_state, group=None, options=None
     ):
+        # type: (Callable, Callable, FrameWaypoints, RobotCellState, Optional[str], Optional[Dict]) -> None
         """Asynchronous handler of MoveIt cartesian motion planner service.
 
         :class:`compas_fab.robots.FrameWaypoints` are converted to :class:`compas_fab.backends.ros.messages.Pose` that is native to ROS communication
 
         """
+        planner = self  # type: MoveItPlanner
+        client = planner.client  # type: RosClient
 
         joints = options["joints"]
 
         header = Header(frame_id=options["base_link"])
 
         # Convert compas_fab.robots.FrameWaypoints to list of Pose for ROS
-        list_of_pose = [Pose.from_frame(frame) for frame in waypoints.target_frames]
-
-        joint_state = JointState(
-            header=header, name=start_configuration.joint_names, position=start_configuration.joint_values
-        )
-        start_state = RobotState(joint_state, MultiDOFJointState(header=header), is_diff=True)
-
-        if options.get("attached_collision_meshes"):
-            for acm in options["attached_collision_meshes"]:
-                aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
-                start_state.attached_collision_objects.append(aco)
-
-        # Filter needs to happen after all objects have been added
-        start_state.filter_fields_for_distro(self.client.ros_distro)
+        target_frames = waypoints.target_frames
+        pcf_frames = client.robot_cell.target_frames_to_pcf(start_state, target_frames, waypoints.target_mode, group)
+        list_of_pose = [Pose.from_frame(frame) for frame in pcf_frames]
+
+        # We are calling the synchronous function here for simplicity.
+        planner.set_robot_cell_state(start_state)
+        planning_scene = planner.get_planning_scene()  # type: PlanningScene
+        start_state = planning_scene.robot_state
+
+        # start_configuration = start_state.robot_configuration
+        # joint_state = JointState(
+        #     header=header, name=start_configuration.joint_names, position=start_configuration.joint_values
+        # )
+        # start_state = RobotState(joint_state, MultiDOFJointState(header=header), is_diff=True)
+
+        # if options.get("attached_collision_meshes"):
+        #     for acm in options["attached_collision_meshes"]:
+        #         aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
+        #         start_state.attached_collision_objects.append(aco)
 
         path_constraints = convert_constraints_to_rosmsg(options.get("path_constraints"), header)
 
@@ -154,17 +176,18 @@ def plan_cartesian_motion_with_frame_waypoints_async(
 
         def response_handler(response):
             try:
+                print("Error Code:", response.error_code)
                 trajectory = convert_trajectory(
                     joints, response.solution, response.start_state, response.fraction, None, response
                 )
                 callback(trajectory)
             except Exception as e:
                 errback(e)
 
-        self.GET_CARTESIAN_PATH(self.client, request, response_handler, errback)
+        self.GET_CARTESIAN_PATH(client, request, response_handler, errback)
 
     def plan_cartesian_motion_with_point_axis_waypoints_async(
-        self, callback, errback, waypoints, start_configuration=None, group=None, options=None
+        self, callback, errback, waypoints, start_state=None, group=None, options=None
     ):
         """Asynchronous handler of MoveIt cartesian motion planner service.
 

src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py

@@ -39,7 +39,7 @@ class MoveItPlanMotion(PlanMotion):
         "/plan_kinematic_path", "GetMotionPlan", MotionPlanRequest, MotionPlanResponse, validate_response
     )
 
-    def plan_motion(self, robot, target, start_configuration=None, group=None, options=None):
+    def plan_motion(self, target, start_configuration=None, group=None, options=None):
         """Calculates a motion path.
 
         Parameters
@@ -87,6 +87,10 @@ def plan_motion(self, robot, target, start_configuration=None, group=None, optio
         :class:`compas_fab.robots.JointTrajectory`
             The calculated trajectory.
         """
+        planner = self  # type: MoveItPlanner
+        client = planner.client  # type: RosClient
+        robot = client.robot  # type: Robot
+
         options = options or {}
         kwargs = {}
         kwargs["target"] = target

src/compas_fab/backends/ros/backend_features/move_it_reset_planning_scene.py

@@ -8,7 +8,7 @@
 from compas_fab.backends.ros.messages import ApplyPlanningSceneRequest
 from compas_fab.backends.ros.messages import ApplyPlanningSceneResponse
 from compas_fab.backends.ros.messages import CollisionObject
-from compas_fab.backends.ros.messages import PlanningScene
+from compas_fab.backends.ros.messages import PlanningScene  # noqa: F401
 from compas_fab.backends.ros.service_description import ServiceDescription
 
 __all__ = [

src/compas_fab/robots/state.py

@@ -15,6 +15,7 @@
         from compas_fab.robots import Robot  # noqa: F401
         from compas_robots import Configuration  # noqa: F401
         from compas_robots import RobotModel  # noqa: F401
+        from compas_fab.robots import RobotCell  # noqa: F401
 
 __all__ = [
     "RigidBodyState",