Skip to content

Commit

Permalink
- refactor
Browse files Browse the repository at this point in the history 
- updated closest point to plane function
  • Loading branch information
@kaviyachandran
kaviyachandran committed Apr 16, 2024
1 parent 17ec220 commit 5162e3e
Show file tree
Hide file tree
Showing 7 changed files with 99 additions and 200 deletions.
43 changes: 12 additions & 31 deletions src/giskardpy/casadi_wrapper.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down
4 changes: 2 additions & 2 deletions src/giskardpy/casadi_wrapper.pyi
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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: ...
Expand Down
60 changes: 24 additions & 36 deletions src/giskardpy/goals/align_to_push_door.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand All @@ -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')
110 changes: 22 additions & 88 deletions src/giskardpy/goals/pre_push_door.py
View file
Original file line number Diff line number Diff line change
@@ -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

Expand All @@ -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,
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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)
Expand All @@ -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)

Expand Down
4 changes: 2 additions & 2 deletions src/giskardpy/python_interface/old_python_interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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,
Expand Down
Loading

0 comments on commit 5162e3e

Please sign in to comment.