def update(self):
markers = []
robot = self.get_robot()
get_fk = robot.get_fk_pose
links = [x for x in self.get_robot().get_link_names() if robot.has_link_visuals(x)]
for i, link_name in enumerate(links):
marker = robot.link_as_marker(link_name)
if marker is None:
continue
marker.header.frame_id = self.robot_base
marker.action = Marker.ADD
marker.id = int(hashlib.md5(link_name).hexdigest()[:6],
16) # FIXME find a better way to give the same link the same id
marker.ns = u'planning_visualization'
marker.header.stamp = rospy.Time()
origin = robot.get_urdf_link(link_name).visual.origin
fk = get_fk(self.robot_base, link_name).pose
if origin is not None:
marker.pose.position = Point(*origin.xyz)
marker.pose.orientation = Quaternion(*quaternion_from_euler(*origin.rpy))
marker.pose = kdl_to_pose(pose_to_kdl(fk) * pose_to_kdl(marker.pose))
else:
marker.pose = fk
markers.append(marker)
self.publisher.publish(markers)
return py_trees.common.Status.SUCCESS
def test_boxy_fk1(self, parsed_boxy, js):
kdl = KDL(boxy_urdf())
root = u'base_footprint'
tips = [u'left_gripper_tool_frame', u'right_gripper_tool_frame']
for tip in tips:
kdl_r = kdl.get_robot(root, tip)
kdl_fk = kdl_to_pose(kdl_r.fk(js))
mjs = {}
for joint_name, position in js.items():
mjs[joint_name] = SingleJointState(joint_name, position)
parsed_boxy.joint_state = mjs
symengine_fk = parsed_boxy.get_fk_pose(root, tip).pose
compare_poses(kdl_fk, symengine_fk)
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 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 update(self):
markers = []
time_stamp = rospy.Time()
objects_dict = self.get_world().get_objects()
# If objects were added, update the namespace and objects
if self.has_environment_changed():
self.set_full_link_names_for_objects()
# Creating of marker for every link in an object
for object_name, object in objects_dict.items():
for link_name in object.get_link_names():
if object.has_link_visuals(link_name):
# Simple objects (containing only one link) are skipped, since they are already managed
# in plugin_pybullet.py and as marker encoded with the function as_marker_msg from urdf_object.py
if link_name == object_name and len(
object.get_link_names()) == 1:
continue
# More complex objects will be published here:
else:
marker = object.link_as_marker(link_name)
if marker is None:
continue
marker.header.frame_id = self.map_frame
id_str = self.get_id_str(object_name, link_name)
marker.id = int(
hashlib.md5(id_str).hexdigest()[:6], 16
) # FIXME find a better way to give the same link the same id
self.ids.add(marker.id)
marker.ns = self.marker_namespace
marker.header.stamp = time_stamp
try:
full_link_name = self.links_full_frame_name[
link_name]
except KeyError:
continue
pose = lookup_pose(self.map_frame, full_link_name).pose
if object.has_non_identity_visual_offset(link_name):
marker.pose = kdl_to_pose(
pose_to_kdl(pose) * pose_to_kdl(marker.pose))
else:
marker.pose = pose
markers.append(marker)
self.publisher.publish(markers)
if self.ensure_publish:
rospy.sleep(0.1)
return py_trees.common.Status.SUCCESS
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 test_pr2_fk1(self, parsed_pr2, js):
"""
:type parsed_pr2: Robot
:type js:
:return:
"""
kdl = KDL(pr2_urdf())
root = u'base_link'
tips = [u'l_gripper_tool_frame', u'r_gripper_tool_frame']
for tip in tips:
kdl_r = kdl.get_robot(root, tip)
kdl_fk = kdl_to_pose(kdl_r.fk(js))
mjs = {}
for joint_name, position in js.items():
mjs[joint_name] = SingleJointState(joint_name, position)
parsed_pr2.joint_state = mjs
symengine_fk = parsed_pr2.get_fk_pose(root, tip).pose
compare_poses(kdl_fk, symengine_fk)
def update(self):
markers = []
time_stamp = rospy.Time()
robot = self.get_robot()
get_fk = robot.get_fk_pose
links = [
x for x in self.get_robot().get_link_names()
if robot.has_link_visuals(x)
]
for i, link_name in enumerate(links):
marker = robot.link_as_marker(link_name)
if marker is None:
continue
marker.header.frame_id = self.robot_base
marker.action = Marker.ADD
marker.id = int(
hashlib.md5(link_name.encode('utf-8')).hexdigest()[:6], 16
) # FIXME find a better way to give the same link the same id
self.ids.add(marker.id)
marker.ns = u'planning_visualization'
marker.header.stamp = time_stamp
fk = get_fk(self.robot_base, link_name).pose
if robot.has_non_identity_visual_offset(link_name):
marker.pose = kdl_to_pose(
pose_to_kdl(fk) * pose_to_kdl(marker.pose))
else:
marker.pose = fk
markers.append(marker)
self.publisher.publish(markers)
if self.ensure_publish:
rospy.sleep(0.1)
return py_trees.common.Status.SUCCESS
def homo_matrix_to_pose(m):
return kdl_to_pose(np_to_kdl(m))
