def set_cart_goal_test():
giskard = GiskardWrapper()
poseStamp = lookup_pose("odom_combined", "iai_kitchen/kitchen_island_left_lower_drawer_handle")
h_g = np_to_kdl(np.array([[-1, 0, 0, 0],
[0, 0, 1, 0],
[0, 1, 0, 0],
[0, 0, 0, 1]]))
pose = pose_to_kdl(poseStamp.pose)
pose = pose * h_g
poseStamp.pose = kdl_to_pose(pose)
giskard.set_cart_goal("odom_combined", "r_gripper_tool_frame", poseStamp)
giskard.allow_all_collisions()
giskard.plan_and_execute()
def collisions_to_closest_point(self, collisions, min_allowed_distance):
"""
:param collisions: (robot_link, body_b, link_b) -> ContactInfo
:type collisions: dict
:param min_allowed_distance: (robot_link, body_b, link_b) -> min allowed distance
:type min_allowed_distance: dict
:return: robot_link -> ClosestPointInfo of closest thing
:rtype: dict
"""
closest_point = KeyDefaultDict(lambda k: ClosestPointInfo(
(10, 0, 0), (0, 0, 0), 100, 0.0, k, '', '', (1, 0, 0), k))
root_T_map = self.robot.root_T_map
root = self.robot.get_root()
fk = self.robot.get_fk_np
for key, contact_info in collisions.items(
): # type: ((str, str, str), ContactInfo)
if contact_info is None:
continue
link1 = key[0]
link_T_root = np_to_kdl(fk(link1, root))
a_in_robot_root = link_T_root * root_T_map * PyKDL.Vector(
*contact_info.position_on_a)
b_in_robot_root = root_T_map * PyKDL.Vector(
*contact_info.position_on_b)
n_in_robot_root = root_T_map.M * PyKDL.Vector(
*contact_info.contact_normal_on_b)
try:
cpi = ClosestPointInfo(a_in_robot_root, b_in_robot_root,
contact_info.contact_distance,
min_allowed_distance[key], key[0],
key[1], key[2], n_in_robot_root, key)
except KeyError:
continue
if link1 in closest_point:
closest_point[link1] = min(closest_point[link1],
cpi,
key=lambda x: x.contact_distance)
else:
closest_point[link1] = cpi
return closest_point
def collisions_to_closest_point(self, collisions, min_allowed_distance):
"""
:param collisions: (robot_link, body_b, link_b) -> ContactInfo
:type collisions: dict
:param min_allowed_distance: (robot_link, body_b, link_b) -> min allowed distance
:type min_allowed_distance: dict
:return: robot_link -> ClosestPointInfo of closest thing
:rtype: dict
"""
closest_point = super(PyBulletWorld, self).collisions_to_closest_point(
collisions, min_allowed_distance)
for key, cpi in closest_point.items(): # type: (str, ClosestPointInfo)
if self.__should_flip_contact_info(collisions[cpi.old_key]):
root_T_link = np_to_kdl(
self.robot.get_fk_np(self.robot.get_root(), cpi.link_a))
b_link = PyKDL.Vector(*cpi.position_on_a)
a_root = PyKDL.Vector(*cpi.position_on_b)
b_root = root_T_link * b_link
a_link = root_T_link.Inverse() * a_root
closest_point[key] = ClosestPointInfo(
a_link, b_root, cpi.contact_distance, cpi.min_dist,
cpi.link_a, cpi.body_b, cpi.link_b,
-np.array(cpi.contact_normal), key)
return closest_point
def homo_matrix_to_pose(m):
return kdl_to_pose(np_to_kdl(m))