From 5162e3e98670b114458a445063829787309005f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C2=A8kaviyachandran=C2=A8?= Date: Tue, 16 Apr 2024 15:49:23 +0200 Subject: [PATCH] - refactor - updated closest point to plane function --- src/giskardpy/casadi_wrapper.py | 43 ++----- src/giskardpy/casadi_wrapper.pyi | 4 +- src/giskardpy/goals/align_to_push_door.py | 60 ++++------ src/giskardpy/goals/pre_push_door.py | 110 ++++-------------- .../python_interface/old_python_interface.py | 4 +- .../python_interface/python_interface.py | 4 +- test/test_integration_pr2.py | 74 ++++++------ 7 files changed, 99 insertions(+), 200 deletions(-) diff --git a/src/giskardpy/casadi_wrapper.py b/src/giskardpy/casadi_wrapper.py index 63649ead40..840c87986d 100644 --- a/src/giskardpy/casadi_wrapper.py +++ b/src/giskardpy/casadi_wrapper.py @@ -1996,42 +1996,23 @@ def distance_point_to_line(frame_P_point, frame_P_line_point, frame_V_line_direc return distance -def distance_point_to_rectangular_surface(frame_P_current, frame_P_bottom_left, frame_P_bottom_right, frame_P_top_left): +def distance_point_to_plane(frame_P_current, frame_P_bottom_left, frame_P_bottom_right, frame_P_top_left) \ + -> tuple[Expression, Point3]: frame_P_current = Point3(frame_P_current) frame_P_bottom_left = Point3(frame_P_bottom_left) frame_P_bottom_right = Point3(frame_P_bottom_right) frame_P_top_left = Point3(frame_P_top_left) - ab_vec = frame_P_bottom_right - frame_P_bottom_left # along y axis - ac_vec = frame_P_top_left - frame_P_bottom_left # along z axis - ap_vec = frame_P_current - frame_P_bottom_left - - ab_len = norm(ab_vec) - ac_len = norm(ac_vec) - ab_unit = ab_vec / ab_len - ac_unit = ac_vec / ac_len - - proj_pt = [ab_unit.dot(ap_vec / ab_len), ac_unit.dot(ap_vec / ac_len)] - proj_pt[0] = limit(proj_pt[0], lower_limit=0.0, upper_limit=1.0) - proj_pt[1] = limit(proj_pt[1], lower_limit=0.0, upper_limit=1.0) - nearest = frame_P_bottom_left + proj_pt[0] * ab_vec + proj_pt[1] * ac_vec - - # n_vec = cross(ab_vec, ac_vec) - # n_unit = n_vec / np.linalg.norm - # proj_pt = ap_vec - dot(ap_vec, n_unit)*n_unit - # - # - # transformed_point = ca.mtimes(ca.horzcat(ab_unit, ac_unit).T, ap_vec) - # - # y_pnt = transformed_point[0]/ab_len - # z_pnt = transformed_point[1]/ac_len - # - # y_pnt = limit(y_pnt, lower_limit=0.0, upper_limit=1.0) - # z_pnt = limit(z_pnt, lower_limit=0.0, upper_limit=1.0) - - # nearest = frame_P_bottom_left + np.array([0, y_pnt*ab_vec[1], z_pnt*ac_vec[2]]) - dist = norm(nearest - frame_P_current) - return dist, nearest + ab = frame_P_bottom_right - frame_P_bottom_left + ac = frame_P_top_left - frame_P_bottom_left + + normal = cross(ab, ac) + d = normal.dot(frame_P_bottom_left) + unit_normal = normal / norm(normal) + + unit_normal.scale(d) + nearest = frame_P_current + unit_normal + return norm(nearest-frame_P_current), nearest def angle_between_vector(v1, v2): diff --git a/src/giskardpy/casadi_wrapper.pyi b/src/giskardpy/casadi_wrapper.pyi index bec39d6134..fbf4c88c17 100644 --- a/src/giskardpy/casadi_wrapper.pyi +++ b/src/giskardpy/casadi_wrapper.pyi @@ -834,8 +834,8 @@ def distance_point_to_line_segment(frame_P_current: Point3, frame_P_line_start: def distance_point_to_line(frame_P_point: Point3, frame_P_line_point: Point3, frame_V_line_direction: Vector3) \ -> Expression: ... -def distance_point_to_rectangular_surface(frame_P_current: Point3, frame_P_bottom_left: Point3, - frame_P_bottom_right: Point3, frame_P_top_left: Point3)-> \ +def distance_point_to_plane(frame_P_current: Point3, frame_P_bottom_left: Point3, + frame_P_bottom_right: Point3, frame_P_top_left: Point3) -> \ Tuple[Expression, Point3]: ... def angle_between_vector(v1: Vector3, v2: Vector3) -> Expression: ... diff --git a/src/giskardpy/goals/align_to_push_door.py b/src/giskardpy/goals/align_to_push_door.py index d2e5024cf6..1cdab02b3e 100644 --- a/src/giskardpy/goals/align_to_push_door.py +++ b/src/giskardpy/goals/align_to_push_door.py @@ -83,9 +83,10 @@ def __init__(self, root_P_intermediate_point.point.y = root_T_door.pose.position.y root_P_intermediate_point.point.z = root_T_door.pose.position.z if root_V_object_rotation_axis.y == 1: - root_P_intermediate_point.point.z = root_P_intermediate_point.point.z + self.door_height + # 3/4 of the height as the tip has to be a little further inside the object + root_P_intermediate_point.point.z = root_P_intermediate_point.point.z + self.door_height*3/4 elif root_V_object_rotation_axis.z == 1: - root_P_intermediate_point.point.y = root_P_intermediate_point.point.y + self.door_length + root_P_intermediate_point.point.y = root_P_intermediate_point.point.y + self.door_length*3/4 # point w.r.t door door_P_intermediate_point = cas.dot(cas.TransMatrix(door_T_root), cas.Point3(root_P_intermediate_point)) @@ -99,39 +100,26 @@ def __init__(self, root_P_top = cas.dot(cas.TransMatrix(root_T_door_expr), cas.Point3(door_rotated_P_top)) minimum_angle_to_push_door = 0.349 - # door_open = self.object_joint_angle >= minimum_angle_to_push_door door_open = cas.greater_equal(self.object_joint_angle, minimum_angle_to_push_door) - try: - if symbol_manager.evaluate_expr(expr=door_open): - god_map.debug_expression_manager.add_debug_expression('goal_point', cas.Point3(root_P_top), - color=ColorRGBA(0, 0.5, 0.5, 1)) - - god_map.debug_expression_manager.add_debug_expression('root_V_grasp_axis', root_V_tip_grasp_axis) - god_map.debug_expression_manager.add_debug_expression('root_V_object_axis', root_V_object_rotation_axis) - align_to_push_task = self.create_and_add_task('align_to_push_door') - align_to_push_task.add_point_goal_constraints(frame_P_current=root_T_tip.to_position(), - frame_P_goal=cas.Point3(root_P_top), - reference_velocity=self.reference_linear_velocity, - weight=self.weight) - - align_to_push_task.add_vector_goal_constraints(frame_V_current=cas.Vector3(root_V_tip_grasp_axis), - frame_V_goal=cas.Vector3(root_V_object_rotation_axis), - reference_velocity=self.reference_angular_velocity, - weight=self.weight) - - # align_to_push_task.start_condition = door_opened_monitor - # align_to_push_task.end_condition = tip_aligned_monitor - self.connect_monitors_to_all_tasks(start_condition, hold_condition, end_condition) - else: - raise GoalInitalizationException('Goal cant be initialized') - - except GoalInitalizationException as e: - logging.loginfo(f'{str(e)}.Failed to initialise {self.__class__.__name__} goal as ' - f'the door is not open') - - # door_opened_monitor = ExpressionMonitor(name='door opened', stay_true=True, start_condition=start_condition) - # self.add_monitor(door_opened_monitor) - # door_opened_monitor.expression = door_open - - + if symbol_manager.evaluate_expr(expr=door_open): + god_map.debug_expression_manager.add_debug_expression('goal_point', cas.Point3(root_P_top), + color=ColorRGBA(0, 0.5, 0.5, 1)) + + god_map.debug_expression_manager.add_debug_expression('root_V_grasp_axis', root_V_tip_grasp_axis) + god_map.debug_expression_manager.add_debug_expression('root_V_object_axis', root_V_object_rotation_axis) + align_to_push_task = self.create_and_add_task('align_to_push_door') + align_to_push_task.add_point_goal_constraints(frame_P_current=root_T_tip.to_position(), + frame_P_goal=cas.Point3(root_P_top), + reference_velocity=self.reference_linear_velocity, + weight=self.weight) + + align_to_push_task.add_vector_goal_constraints(frame_V_current=cas.Vector3(root_V_tip_grasp_axis), + frame_V_goal=cas.Vector3(root_V_object_rotation_axis), + reference_velocity=self.reference_angular_velocity, + weight=self.weight) + + self.connect_monitors_to_all_tasks(start_condition, hold_condition, end_condition) + else: + raise GoalInitalizationException(f'Goal cant be initialized. Failed to initialise {self.__class__.__name__}' + 'goal as the door is not open') diff --git a/src/giskardpy/goals/pre_push_door.py b/src/giskardpy/goals/pre_push_door.py index 62c2017b93..07972e50e7 100644 --- a/src/giskardpy/goals/pre_push_door.py +++ b/src/giskardpy/goals/pre_push_door.py @@ -1,13 +1,9 @@ from typing import Optional -import numpy as np - -from geometry_msgs.msg import Vector3Stamped, PointStamped, Quaternion -from std_msgs.msg import ColorRGBA +from geometry_msgs.msg import Vector3Stamped, PointStamped from giskardpy import casadi_wrapper as cas from giskardpy.goals.goal import Goal from giskardpy.god_map import god_map -from giskardpy.monitors.monitors import ExpressionMonitor from giskardpy.tasks.task import WEIGHT_BELOW_CA from giskardpy.utils import tfwrapper as tf @@ -20,10 +16,9 @@ def __init__(self, door_object: str, door_height: float, door_length: float, + door_depth: float, tip_gripper_axis: Vector3Stamped, root_V_object_rotation_axis: Vector3Stamped, - # normal is along x axis, plane is located along y-z axis - root_V_object_normal: Vector3Stamped, object_joint_name: str, root_group: Optional[str] = None, tip_group: Optional[str] = None, @@ -47,8 +42,6 @@ def __init__(self, tip_gripper_axis.vector = tf.normalize(tip_gripper_axis.vector) root_V_object_rotation_axis.header.frame_id = self.root root_V_object_rotation_axis.vector = tf.normalize(root_V_object_rotation_axis.vector) - root_V_object_normal.header.frame_id = self.root - root_V_object_normal.vector = tf.normalize(root_V_object_normal.vector) self.tip_gripper_axis = tip_gripper_axis self.object_rotation_axis = root_V_object_rotation_axis @@ -57,6 +50,7 @@ def __init__(self, self.door_height = door_height self.door_length = door_length + self.door_depth = door_depth self.reference_linear_velocity = reference_linear_velocity self.reference_angular_velocity = reference_angular_velocity @@ -65,69 +59,39 @@ def __init__(self, name = f'{self.__class__.__name__}/{self.tip}/{self.door_object}' super().__init__(name) - self.axis = {0: "x", 1: "y", 2: "z"} - - self.rotation_axis = np.argmax(np.abs([root_V_object_rotation_axis.vector.x, - root_V_object_rotation_axis.vector.y, - root_V_object_rotation_axis.vector.z])) - self.normal_axis = np.argmax([root_V_object_normal.vector.x, - root_V_object_normal.vector.y, - root_V_object_normal.vector.z]) root_T_tip = god_map.world.compose_fk_expression(self.root, self.tip) root_P_bottom_left = PointStamped() # A root_P_bottom_right = PointStamped() # B root_P_top_left = PointStamped() # C - door_P_bottom_left = PointStamped() - - min_y = 0 - max_y = 0 - min_z = 0 - max_z = 0 - - # Plane lies in Y-Z axis - if self.axis[int(self.rotation_axis)] == 'y': - min_y = -1 / 2 - max_y = 1 / 2 - min_z = 1 / 4 - max_z = 3 / 4 - elif self.axis[int(self.rotation_axis)] == 'z': - min_y = 1 / 4 - max_y = 3 / 4 - min_z = -1 / 2 - max_z = 1 / 2 root_T_door = god_map.world.compute_fk_pose(self.root, self.door_object) root_P_bottom_left.header.frame_id = self.root - root_P_bottom_left.point.x = root_T_door.pose.position.x - root_P_bottom_left.point.y = root_T_door.pose.position.y + self.door_length * max_y - root_P_bottom_left.point.z = root_T_door.pose.position.z + self.door_height * min_z + root_P_bottom_left.point.x = root_T_door.pose.position.x + self.door_depth + root_P_bottom_left.point.y = root_T_door.pose.position.y + self.door_length/2 + root_P_bottom_left.point.z = root_T_door.pose.position.z root_P_bottom_right.header.frame_id = self.root - root_P_bottom_right.point.x = root_T_door.pose.position.x - root_P_bottom_right.point.y = root_T_door.pose.position.y + self.door_length * min_y - root_P_bottom_right.point.z = root_T_door.pose.position.z + self.door_height * min_z + root_P_bottom_right.point.x = root_T_door.pose.position.x + self.door_depth + root_P_bottom_right.point.y = root_T_door.pose.position.y + root_P_bottom_right.point.z = root_T_door.pose.position.z root_P_top_left.header.frame_id = self.root - root_P_top_left.point.x = root_T_door.pose.position.x - root_P_top_left.point.y = root_T_door.pose.position.y + self.door_length * max_y - root_P_top_left.point.z = root_T_door.pose.position.z + self.door_height * max_z - - # dishwasher dimensions - 0.0200, 0.49, 0.6 - # C-------D - # | | - # A-------B + root_P_top_left.point.x = root_T_door.pose.position.x + self.door_depth + root_P_top_left.point.y = root_T_door.pose.position.y + self.door_length/2 + root_P_top_left.point.z = root_T_door.pose.position.z + self.door_height/2 + root_Pose_tip = god_map.world.compute_fk_pose(self.root, self.tip) root_P_tip = PointStamped() root_P_tip.header.frame_id = self.root root_P_tip.point.x = root_Pose_tip.pose.position.x root_P_tip.point.y = root_Pose_tip.pose.position.y root_P_tip.point.z = root_Pose_tip.pose.position.z - dist, root_P_nearest = cas.distance_point_to_rectangular_surface(cas.Point3(root_P_tip), - cas.Point3(root_P_bottom_left), - cas.Point3(root_P_bottom_right), - cas.Point3(root_P_top_left)) + dist, root_P_nearest = cas.distance_point_to_plane(cas.Point3(root_P_tip), + cas.Point3(root_P_bottom_left), + cas.Point3(root_P_bottom_right), + cas.Point3(root_P_top_left)) door_T_root = god_map.world.compute_fk_pose(self.door_object, self.root) door_P_nearest = cas.dot(cas.TransMatrix(door_T_root), root_P_nearest) @@ -137,49 +101,19 @@ def __init__(self, rot_mat = cas.RotationMatrix.from_axis_angle(cas.Vector3(object_V_object_rotation_axis), self.object_joint_angle) - door_P_rotated_point = cas.dot(rot_mat, door_P_nearest) + door_rotated_P_nearest = cas.dot(cas.TransMatrix(rot_mat), door_P_nearest) - root_P_rotated_point = cas.dot(cas.TransMatrix(root_T_door), cas.Point3(door_P_rotated_point)) + root_P_nearest_in_rotated_door = cas.dot(cas.TransMatrix(root_T_door), cas.Point3(door_rotated_P_nearest)) - god_map.debug_expression_manager.add_debug_expression('goal_point_on_plane', cas.Point3(root_P_rotated_point)) + god_map.debug_expression_manager.add_debug_expression('goal_point_on_plane', + cas.Point3(root_P_nearest_in_rotated_door)) god_map.debug_expression_manager.add_debug_expression('A', cas.Point3(root_P_bottom_left)) god_map.debug_expression_manager.add_debug_expression('B', cas.Point3(root_P_bottom_right)) god_map.debug_expression_manager.add_debug_expression('C', cas.Point3(root_P_top_left)) - door_P_bottom_left.header.frame_id = self.door_object - door_P_bottom_left.point.y = self.door_length * max_y - door_P_bottom_left.point.z = self.door_height * min_z - - door_rotated_P_bottom_left = cas.dot(rot_mat, cas.Point3(door_P_bottom_left)) - root_P_bottom_left_rotated = cas.dot(cas.TransMatrix(root_T_door), cas.Point3(door_rotated_P_bottom_left)) - root_P_tip = cas.Point3(root_P_tip) - root_V_object_normal = cas.Vector3(root_V_object_normal) - - root_P_bottom_left = cas.Point3(root_P_bottom_left) - - d1 = cas.abs(root_V_object_normal.x * (root_P_tip.x - root_P_bottom_left_rotated.x) + - root_V_object_normal.y * (root_P_tip.y - root_P_bottom_left_rotated.y) + - root_V_object_normal.z * (root_P_tip.z - root_P_bottom_left_rotated.z)) - - d2 = cas.abs(root_V_object_normal.x * (root_P_tip.x - root_P_bottom_left.x) + - root_V_object_normal.y * (root_P_tip.y - root_P_bottom_left.y) + - root_V_object_normal.z * (root_P_tip.z - root_P_bottom_left.z)) - - d = cas.abs(root_V_object_normal.x * (root_P_bottom_left_rotated.x - root_P_bottom_left.x) + - root_V_object_normal.y * (root_P_bottom_left_rotated.y - root_P_bottom_left.y) + - root_V_object_normal.z * (root_P_bottom_left_rotated.z - root_P_bottom_left.z)) - - # check if the tip point is between the two planes (closed and rotated door). - # this is to make sure if the gripper is aligned appropriately - gripper_aligned = cas.equal(d1 + d2, d) - gripper_aligned_monitor = ExpressionMonitor(name='gripper aligned', stay_true=True, - start_condition=start_condition) - self.add_monitor(gripper_aligned_monitor) - gripper_aligned_monitor.expression = gripper_aligned - push_door_task = self.create_and_add_task('pre push door') push_door_task.add_point_goal_constraints(frame_P_current=root_T_tip.to_position(), - frame_P_goal=cas.Point3(root_P_rotated_point), + frame_P_goal=cas.Point3(root_P_nearest_in_rotated_door), reference_velocity=self.reference_linear_velocity, weight=self.weight) diff --git a/src/giskardpy/python_interface/old_python_interface.py b/src/giskardpy/python_interface/old_python_interface.py index 0a0d727ac9..8f9fb29740 100644 --- a/src/giskardpy/python_interface/old_python_interface.py +++ b/src/giskardpy/python_interface/old_python_interface.py @@ -540,10 +540,10 @@ def set_pre_push_door_goal(self, door_object: str, door_height: float, door_length: float, + door_depth: float, tip_gripper_axis: Vector3Stamped, root_V_object_rotation_axis: Vector3Stamped, # normal is along x axis, plane is located along y-z axis - root_V_object_normal: Vector3Stamped, object_joint_name: str, reference_linear_velocity: Optional[float] = None, reference_angular_velocity: Optional[float] = None, @@ -564,9 +564,9 @@ def set_pre_push_door_goal(self, door_object=door_object, door_height=door_height, door_length=door_length, + door_depth=door_depth, tip_gripper_axis=tip_gripper_axis, root_V_object_rotation_axis=root_V_object_rotation_axis, - root_V_object_normal=root_V_object_normal, object_joint_name=object_joint_name, reference_linear_velocity=reference_linear_velocity, reference_angular_velocity=reference_angular_velocity, diff --git a/src/giskardpy/python_interface/python_interface.py b/src/giskardpy/python_interface/python_interface.py index 9b4153df11..7cc838c219 100644 --- a/src/giskardpy/python_interface/python_interface.py +++ b/src/giskardpy/python_interface/python_interface.py @@ -717,9 +717,9 @@ def add_pre_push_door(self, door_object: str, door_height: float, door_length: float, + door_depth: float, tip_gripper_axis: Vector3Stamped, root_V_object_rotation_axis: Vector3Stamped, - root_V_object_normal: Vector3Stamped, object_joint_name: str, weight: float, tip_group: Optional[str] = None, @@ -747,9 +747,9 @@ def add_pre_push_door(self, door_object=door_object, door_height=door_height, door_length=door_length, + door_depth=door_depth, tip_gripper_axis=tip_gripper_axis, root_V_object_rotation_axis=root_V_object_rotation_axis, - root_V_object_normal=root_V_object_normal, object_joint_name=object_joint_name, tip_group=tip_group, root_group=root_group, diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index bf351d4fbb..43da60ece5 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -1620,7 +1620,7 @@ def test_push_open_dishwasher(self, kitchen_setup: PR2TestWrapper): door_joint = 'sink_area_dish_washer_door_joint' kitchen_setup.register_group(door_obj, kitchen_setup.default_env_name, door_name) # root link of the objects to avoid collision - kitchen_setup.set_env_state({'sink_area_dish_washer_door_joint': 0}) + kitchen_setup.set_env_state({'sink_area_dish_washer_door_joint': np.pi/8}) tip_grasp_axis = Vector3Stamped() tip_grasp_axis.header.frame_id = hand tip_grasp_axis.vector.y = 1 @@ -1639,44 +1639,40 @@ def test_push_open_dishwasher(self, kitchen_setup: PR2TestWrapper): object_rotation_axis=object_rotation_axis) kitchen_setup.plan_and_execute() - # if result.error.code == GiskardError.SUCCESS: - # object_normal = Vector3Stamped() - # object_normal.header.frame_id = door_name - # object_normal.vector.x = 1 - # - # # # # close the gripper - # kitchen_setup.set_joint_goal(goal_state={'r_gripper_l_finger_joint': 0.0}) - # - # root_V_object_rotation_axis = Vector3Stamped() - # root_V_object_rotation_axis.header.frame_id = "map" - # root_V_object_rotation_axis.vector.y = -1 - # - # kitchen_setup.set_pre_push_door_goal(root_link=kitchen_setup.default_root, - # tip_link=hand, - # door_object=door_name, - # door_height=0.6, - # door_length=0.49, - # tip_gripper_axis=tip_grasp_axis, - # root_V_object_rotation_axis=root_V_object_rotation_axis, - # object_joint_name=door_joint, - # root_V_object_normal=object_normal) - # - # kitchen_setup.allow_collision(group1=door_obj, group2=kitchen_setup.r_gripper_group) - # kitchen_setup.plan_and_execute() - # - # # Qn to Simon: There is a minor difference compared to the normal drawer scenario, here the robot tip does not have to move - # # with the handle. The force applied by the gripper can just move the object further - # # (if the force is sufficient) - # right_forearm = 'r_forearm' - # kitchen_setup.register_group(right_forearm, - # root_link_group_name=kitchen_setup.robot_name, - # root_link_name='r_forearm_link') - # kitchen_setup.set_open_container_goal(tip_link=hand, - # environment_link=handle_name, - # goal_joint_state=1.3217) - # - # kitchen_setup.allow_collision(group1=door_obj, group2=right_forearm) - # kitchen_setup.plan_and_execute() + object_normal = Vector3Stamped() + object_normal.header.frame_id = door_name + object_normal.vector.x = 1 + + # # # close the gripper + kitchen_setup.set_joint_goal(goal_state={'r_gripper_l_finger_joint': 0.0}) + + root_V_object_rotation_axis = Vector3Stamped() + root_V_object_rotation_axis.header.frame_id = "map" + root_V_object_rotation_axis.vector.y = -1 + + kitchen_setup.set_pre_push_door_goal(root_link=kitchen_setup.default_root, + tip_link=hand, + door_object=door_name, + door_height=0.6, + door_length=0.49, + door_depth=0.02, + tip_gripper_axis=tip_grasp_axis, + root_V_object_rotation_axis=root_V_object_rotation_axis, + object_joint_name=door_joint) + + kitchen_setup.allow_collision(group1=door_obj, group2=kitchen_setup.r_gripper_group) + kitchen_setup.plan_and_execute() + + right_forearm = 'r_forearm' + kitchen_setup.register_group(right_forearm, + root_link_group_name=kitchen_setup.robot_name, + root_link_name='r_forearm_link') + kitchen_setup.set_open_container_goal(tip_link=hand, + environment_link=handle_name, + goal_joint_state=1.3217) + + kitchen_setup.allow_collision(group1=door_obj, group2=right_forearm) + kitchen_setup.plan_and_execute() def test_align_planes1(self, zero_pose: PR2TestWrapper): x_gripper = Vector3Stamped()