def __init__(self, god_map, root, tip, root_normal, tip_normal, weight=HIGH_WEIGHT, gain=3, max_speed=0.5):
"""
:type god_map:
:type root: str
:type tip: str
:type root_normal: Vector3Stamped
:type tip_normal: Vector3Stamped
"""
super(AlignPlanes, self).__init__(god_map)
self.root = root
self.tip = tip
root_normal = convert_dictionary_to_ros_message(u'geometry_msgs/Vector3Stamped', root_normal)
root_normal = transform_vector(self.root, root_normal)
tmp = np.array([root_normal.vector.x, root_normal.vector.y, root_normal.vector.z])
tmp = tmp / np.linalg.norm(tmp)
root_normal.vector = Vector3(*tmp)
tip_normal = convert_dictionary_to_ros_message(u'geometry_msgs/Vector3Stamped', tip_normal)
tip_normal = transform_vector(self.tip, tip_normal)
tmp = np.array([tip_normal.vector.x, tip_normal.vector.y, tip_normal.vector.z])
tmp = tmp / np.linalg.norm(tmp)
tip_normal.vector = Vector3(*tmp)
params = {self.root_normal: root_normal,
self.tip_normal: tip_normal,
self.weight: weight,
self.gain: gain,
self.max_speed: max_speed}
self.save_params_on_god_map(params)
def __init__(self, god_map, tip, goal_point, root=None, pointing_axis=None, weight=HIGH_WEIGHT):
super(Pointing, self).__init__(god_map)
if root is None:
self.root = self.get_robot().get_root()
else:
self.root = root
self.tip = tip
goal_point = convert_dictionary_to_ros_message(u'geometry_msgs/PointStamped', goal_point)
goal_point = transform_point(self.root, goal_point)
if pointing_axis is not None:
pointing_axis = convert_dictionary_to_ros_message(u'geometry_msgs/Vector3Stamped', pointing_axis)
pointing_axis = transform_vector(self.tip, pointing_axis)
else:
pointing_axis = Vector3Stamped()
pointing_axis.header.frame_id = self.tip
pointing_axis.vector.z = 1
tmp = np.array([pointing_axis.vector.x, pointing_axis.vector.y, pointing_axis.vector.z])
tmp = tmp / np.linalg.norm(tmp) # TODO possible /0
pointing_axis.vector = Vector3(*tmp)
params = {self.goal_point: goal_point,
self.pointing_axis: pointing_axis,
self.weight: weight}
self.save_params_on_god_map(params)
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), 1e9, self.default_collision_avoidance_distance, k, '',
(1, 0, 0)))
for key, collision_info in collisions.items(
): # type: ((str, str, str), ContactInfo)
if collision_info is None:
continue
link1 = key[0]
a_in_robot_root = msg_to_list(
transform_point(
self.robot_root,
to_point_stamped(self.map_frame,
collision_info.position_on_a)))
b_in_robot_root = msg_to_list(
transform_point(
self.robot_root,
to_point_stamped(self.map_frame,
collision_info.position_on_b)))
n_in_robot_root = msg_to_list(
transform_vector(
self.robot_root,
to_vector3_stamped(self.map_frame,
collision_info.contact_normal_on_b)))
try:
cpi = ClosestPointInfo(a_in_robot_root, b_in_robot_root,
collision_info.contact_distance,
min_allowed_distance[key], key[0],
u'{} - {}'.format(key[1], key[2]),
n_in_robot_root)
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