def base_pose(self, value):
"""
:type value: Pose
:return:
"""
orientation_vector = np.array([
value.orientation.x, value.orientation.y, value.orientation.z,
value.orientation.w
])
self._base_pose = value
self._base_pose.orientation = Quaternion(
*orientation_vector / np.linalg.norm(orientation_vector))
self.root_T_map = msg_to_kdl(self._base_pose).Inverse()
def cb(self, traj):
"""
:type traj: FollowJointTrajectoryActionGoal
:return:
"""
map_T_odom_original = msg_to_kdl(tf.lookup_pose(
'map', self.odom_frame))
traj = traj.goal
start_time = traj.trajectory.header.stamp
js = JointState()
js.name = traj.trajectory.joint_names
odom_x_id = js.name.index(self.odom_x)
odom_y_id = js.name.index(self.odom_y)
odom_z_id = js.name.index(self.odom_z)
rospy.sleep(start_time - rospy.get_rostime())
dt = traj.trajectory.points[1].time_from_start.to_sec(
) - traj.trajectory.points[0].time_from_start.to_sec()
r = rospy.Rate(1 / dt)
for traj_point in traj.trajectory.points:
odom_T_map = msg_to_kdl(tf.lookup_pose(self.odom_frame, 'map'))
js.header.stamp = start_time + traj_point.time_from_start
js.position = list(traj_point.positions)
js.velocity = traj_point.velocities
odom_x = js.position[odom_x_id]
odom_y = js.position[odom_y_id]
odom_z = js.position[odom_z_id]
odom_original_T_goal = kdl.Frame(kdl.Rotation().RotZ(odom_z),
kdl.Vector(odom_x, odom_y, 0))
map_T_goal = map_T_odom_original * odom_original_T_goal
odom_T_goal = odom_T_map * map_T_goal
js.position[odom_x_id] = odom_T_goal.p[0]
js.position[odom_y_id] = odom_T_goal.p[1]
js.position[odom_z_id] = kdl.Rotation().RotZ(odom_z).GetRot()[2]
self.joint_state_pub.publish(js)
r.sleep()
js.velocity = [0 for _ in js.velocity]
self.joint_state_pub.publish(js)
def detach(self, joint_name, from_obj=None):
if from_obj is None or self.robot.get_name() == from_obj:
# this only works because attached simple objects have joint names equal to their name
p = self.robot.get_fk_pose(self.robot.get_root(), joint_name)
p_map = kdl_to_pose(self.robot.root_T_map.Inverse() *
msg_to_kdl(p))
cut_off_obj = self.robot.detach_sub_tree(joint_name)
else:
raise UnsupportedOptionException(
u'only detach from robot supported')
wo = WorldObject.from_urdf_object(cut_off_obj) # type: WorldObject
wo.base_pose = p_map
self.add_object(wo)
def detach(self, joint_name, from_obj=None):
if joint_name not in self.robot.get_joint_names():
raise UnknownBodyException(u'can\'t detach: {}'.format(joint_name))
if from_obj is None or self.robot.get_name() == from_obj:
# this only works because attached simple objects have joint names equal to their name
p = self.robot.get_fk_pose(self.robot.get_root(), joint_name)
p_map = kdl_to_pose(self.robot.root_T_map.Inverse() * msg_to_kdl(p))
parent_link = self.robot.get_parent_link_of_joint(joint_name)
cut_off_obj = self.robot.detach_sub_tree(joint_name)
logging.loginfo(u'<-- detached {} from link {}'.format(joint_name, parent_link))
else:
raise UnsupportedOptionException(u'only detach from robot supported')
wo = WorldObject.from_urdf_object(cut_off_obj) # type: WorldObject
wo.base_pose = p_map
self.add_object(wo)
def publish_cpi_markers(self, closest_point_infos):
"""
Publishes a string for each ClosestPointInfo in the dict. If the distance is below the threshold, the string
is colored red. If it is below threshold*2 it is yellow. If it is below threshold*3 it is green.
Otherwise no string will be published.
:type closest_point_infos: dict
"""
m = Marker()
m.header.frame_id = self.get_robot().get_root()
m.action = Marker.ADD
m.type = Marker.LINE_LIST
m.id = 1337
# TODO make namespace parameter
m.ns = u'pybullet collisions'
m.scale = Vector3(0.003, 0, 0)
if len(closest_point_infos) > 0:
for collision_info in closest_point_infos.values(
): # type: ClosestPointInfo
red_threshold = collision_info.min_dist
yellow_threshold = collision_info.min_dist * 2
green_threshold = collision_info.min_dist * 3
if collision_info.contact_distance < green_threshold:
root_T_link = self.get_robot().get_fk_pose(
self.get_robot().get_root(), collision_info.link_a)
a__root = msg_to_kdl(root_T_link) * PyKDL.Vector(
*collision_info.position_on_a)
m.points.append(Point(*a__root))
m.points.append(Point(*collision_info.position_on_b))
m.colors.append(ColorRGBA(0, 1, 0, 1))
m.colors.append(ColorRGBA(0, 1, 0, 1))
if collision_info.contact_distance < yellow_threshold:
m.colors[-2] = ColorRGBA(1, 1, 0, 1)
m.colors[-1] = ColorRGBA(1, 1, 0, 1)
if collision_info.contact_distance < red_threshold:
m.colors[-2] = ColorRGBA(1, 0, 0, 1)
m.colors[-1] = ColorRGBA(1, 0, 0, 1)
ma = MarkerArray()
ma.markers.append(m)
self.pub_collision_marker.publish(ma)