Skip to content

Commit

Permalink
work on dual arm tcp planning
Browse files Browse the repository at this point in the history
  • Loading branch information
Victorlouisdg committed Mar 22, 2024
1 parent 8e877c6 commit 746071e
Show file tree
Hide file tree
Showing 7 changed files with 96 additions and 39 deletions.
8 changes: 7 additions & 1 deletion airo_planner/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,12 @@
# but for building and many toolings, you still need to have __init__ files (at least in the root of the package).
# e.g. if you remove this init file and try to build with pip install .
# you won't be able to import the dummy module.
from airo_planner.joint_bounds import JointBoundsType, concatenate_joint_bounds, uniform_symmetric_joint_bounds
from airo_planner.joint_bounds import (
JointBoundsType,
concatenate_joint_bounds,
is_within_bounds,
uniform_symmetric_joint_bounds,
)
from airo_planner.path_conversions import stack_joints
from airo_planner.selection.goal_selection import (
filter_with_distance_to_configurations,
Expand Down Expand Up @@ -32,6 +37,7 @@
"SingleArmOmplPlanner",
"DualArmOmplPlanner",
"JointBoundsType",
"is_within_bounds",
"uniform_symmetric_joint_bounds",
"concatenate_joint_bounds",
"state_from_ompl",
Expand Down
8 changes: 8 additions & 0 deletions airo_planner/exceptions.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
class PlannerError(RuntimeError):
"""Base class for all things that can go wrong during planning.
Mostly intended for problems that can happen, but are not per se
the user's fault, e.g. no valid IK solutions, no valid paths etc.
Note that not all planner problems need to be a subclass of this
error, e.g. you can still use ValueError, AttributeError etc.
"""
22 changes: 9 additions & 13 deletions airo_planner/interfaces.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,8 @@ class SingleArmPlanner(abc.ABC):
"""Base class that defines an interface for single-arm motion planners.
The idea is that the custom settings for each motion planner are provided
through the constructor. After creation the motion planner can then be
and from then on all motion planners can be used
in the same way, e.g. for benchmarking.
through the constructor. After creation all motion planners can then be
used in the same way, e.g. for benchmarking.
"""

def plan_to_joint_configuration(
Expand All @@ -32,15 +30,14 @@ def plan_to_joint_configuration(
A discretized path from the start configuration to the goal
configuration. If no solution could be found, then None is returned.
"""
raise NotImplementedError
raise NotImplementedError("This planner has not implemented planning to joint configurations (yet).")

def plan_to_tcp_pose(
self,
start_configuration: JointConfigurationType,
tcp_pose_in_base: HomogeneousMatrixType,
tcp_pose: HomogeneousMatrixType,
) -> JointPathType | None:
"""TODO"""
raise NotImplementedError
raise NotImplementedError("This planner has not implemented planning to TCP poses (yet).")


class DualArmPlanner(abc.ABC):
Expand Down Expand Up @@ -79,14 +76,13 @@ def plan_to_joint_configuration(
the start_configuration will simply be repeated in the path for that
arm. If no solution could be found, then None is returned.
"""
raise NotImplementedError
raise NotImplementedError("This planner has not implemented planning to joint configurations (yet).")

def plan_to_tcp_pose(
self,
start_configuration_left: JointConfigurationType,
start_configuration_right: JointConfigurationType,
tcp_pose_left_in_base: HomogeneousMatrixType | None,
tcp_pose_right_in_base: HomogeneousMatrixType | None,
tcp_pose_left: HomogeneousMatrixType | None,
tcp_pose_right: HomogeneousMatrixType | None,
) -> JointPathType | None:
"""TODO"""
raise NotImplementedError
raise NotImplementedError("This planner has not implemented planning to TCP poses (yet).")
15 changes: 15 additions & 0 deletions airo_planner/joint_bounds.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,21 @@
JointBoundsType = tuple[JointConfigurationType, JointConfigurationType]


def is_within_bounds(joint_configuration: JointConfigurationType, joint_bounds: JointBoundsType) -> bool:
"""Checks if a joint configuration is within the given bounds.
Args:
joint_configuration: The joint configuration to check.
joint_bounds: A tuple containing the lower and upper bounds for each joint.
Returns:
True if the joint configuration is within the bounds, False otherwise.
"""
lower_bounds, upper_bounds = joint_bounds
is_within = np.all(lower_bounds <= joint_configuration) and np.all(joint_configuration <= upper_bounds)
return bool(is_within)


def uniform_symmetric_joint_bounds(degrees_of_freedom: int, value: float = 2 * np.pi) -> JointBoundsType:
"""Creates joint bounds that are the same for each joint and where the min is the negative of the max.
Expand Down
19 changes: 11 additions & 8 deletions airo_planner/ompl/dual_arm_ompl_planner.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
import numpy as np
from airo_typing import (
HomogeneousMatrixType,
InverseKinematicsFunctionType,
JointConfigurationCheckerType,
JointConfigurationType,
Expand Down Expand Up @@ -209,14 +210,16 @@ def plan_to_joint_configuration(
)
return path

# def plan_to_tcp_pose(
# self,
# start_configuration_left: HomogeneousMatrixType,
# start_configuration_right: HomogeneousMatrixType,
# tcp_pose_left_in_base: HomogeneousMatrixType | None,
# tcp_pose_right_in_base: HomogeneousMatrixType | None,
# ) -> JointPathType | None:
# pass
def plan_to_tcp_pose(
self,
start_configuration_left: HomogeneousMatrixType,
start_configuration_right: HomogeneousMatrixType,
tcp_pose_left: HomogeneousMatrixType | None,
tcp_pose_right: HomogeneousMatrixType | None,
) -> JointPathType | None:
if tcp_pose_left is None and tcp_pose_right is None:
raise ValueError("A goal TCP pose must be specified for at least one of the arms.")
return None

# def _plan_to_tcp_pose_left_arm_only(
# self,
Expand Down
20 changes: 13 additions & 7 deletions airo_planner/ompl/single_arm_ompl_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,11 +16,13 @@
SingleArmPlanner,
choose_shortest_path,
create_simple_setup,
is_within_bounds,
solve_and_smooth_path,
state_to_ompl,
uniform_symmetric_joint_bounds,
)

# TODO move this to airo_typing
# TODO move this to airo_typing?
JointConfigurationsModifierType = Callable[[List[JointConfigurationType]], List[JointConfigurationType]]
JointPathChooserType = Callable[[List[JointPathType]], JointPathType]

Expand Down Expand Up @@ -70,7 +72,7 @@ def __init__(
# Planning parameters
self._degrees_of_freedom = degrees_of_freedom
if joint_bounds is None:
self.joint_bounds = np.full(degrees_of_freedom, -2 * np.pi), np.full(degrees_of_freedom, 2 * np.pi)
self.joint_bounds = uniform_symmetric_joint_bounds(self._degrees_of_freedom)
else:
self.joint_bounds = joint_bounds

Expand All @@ -90,6 +92,11 @@ def _set_start_and_goal_configurations(
raise ValueError("Planner was given an invalid goal configuration.")

# TODO check joint bounds as well
if not is_within_bounds(start_configuration, self.joint_bounds):
raise ValueError("Start configuration is not within the joint bounds.")

if not is_within_bounds(goal_configuration, self.joint_bounds):
raise ValueError("Goal configuration is not within the joint bounds.")

space = self._simple_setup.getStateSpace()
start_state = state_to_ompl(start_configuration, space)
Expand All @@ -111,13 +118,13 @@ def plan_to_joint_configuration(
def plan_to_tcp_pose( # noqa: C901
self,
start_configuration: JointConfigurationType,
tcp_pose_in_base: HomogeneousMatrixType,
tcp_pose: HomogeneousMatrixType,
) -> JointPathType | None:

if self.inverse_kinematics_fn is None:
logger.warning("Planning to TCP pose attempted but inverse_kinematics_fn was provided, returing None.")
return None
raise AttributeError("Inverse kinematics function is required for planning to TCP poses.")

ik_solutions = self.inverse_kinematics_fn(tcp_pose_in_base)
ik_solutions = self.inverse_kinematics_fn(tcp_pose)
ik_solutions = [solution.squeeze() for solution in ik_solutions]
self._ik_solutions = ik_solutions # Save for debugging

Expand Down Expand Up @@ -145,7 +152,6 @@ def plan_to_tcp_pose( # noqa: C901
else:
logger.info(f"Found {len(ik_solutions_valid)}/{len(ik_solutions_within_bounds)} valid solutions.")

# TODO filter the IK solutions -> warn when nothing is left
if self.filter_goal_configurations_fn is not None:
ik_solutions_filtered = self.filter_goal_configurations_fn(ik_solutions_valid)
if len(ik_solutions_filtered) == 0:
Expand Down
43 changes: 33 additions & 10 deletions notebooks/03_dual_arm_planning.ipynb
Original file line number Diff line number Diff line change
Expand Up @@ -287,9 +287,20 @@
"metadata": {},
"outputs": [],
"source": [
"from airo_drake import time_parametrize_toppra\n",
"from airo_drake import time_parametrize_toppra, animate_dual_joint_trajectory\n",
"\n",
"trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)"
"trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n",
"\n",
"animate_dual_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### 2.2 Joints to single arm joints (🦾,🦾) -> (🦾,💤)\n",
"\n",
"Note that you are allow to set one of the goals to `None` to indicate that that arm should not move."
]
},
{
Expand All @@ -298,18 +309,24 @@
"metadata": {},
"outputs": [],
"source": [
"from airo_drake import animate_dual_joint_trajectory\n",
"\n",
"path = planner.plan_to_joint_configuration(tangled_joints_left, tangled_joints_right, home_joints_left, None)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n",
"animate_dual_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### 2.2 Joints to single arm joints (🦾,🦾) -> (🦾,💤)\n",
"\n",
"Note that you are allow to set one of the goals to `None` to indicate that that arm should not move."
"### 2.3 Joints to single TCP pose (🦾,🦾) -> (💤,📐)"
]
},
{
Expand All @@ -318,7 +335,14 @@
"metadata": {},
"outputs": [],
"source": [
"path = planner.plan_to_joint_configuration(tangled_joints_left, tangled_joints_right, home_joints_left, None)"
"from airo_drake import animate_joint_configurations\n",
"\n",
"grasp_pose = np.identity(4)\n",
"grasp_pose[2, 3] = 0.4\n",
"\n",
"grasp_joints = inverse_kinematics_right_fn(grasp_pose)\n",
"\n",
"animate_joint_configurations(scene.meshcat, scene.robot_diagram, scene.arm_right_index, grasp_joints, context=context)"
]
},
{
Expand All @@ -327,8 +351,7 @@
"metadata": {},
"outputs": [],
"source": [
"trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n",
"animate_dual_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory)"
"path = planner.plan_to_tcp_pose(tangled_joints_left, tangled_joints_right, None, grasp_pose)"
]
},
{
Expand Down

0 comments on commit 746071e

Please sign in to comment.