def kitchen_setup(zero_pose):
object_name = u'kitchen'
zero_pose.add_urdf(object_name,
rospy.get_param(u'kitchen_description'),
u'/kitchen/joint_states',
tf.lookup_transform(u'map', u'iai_kitchen/world'),
set_js_topic=u'/kitchen/cram_joint_states')
return zero_pose
def get_frame_in_base_footprint_kdl(self, frame):
"""
:rtype: PyKDL.Frame
"""
# if self.T_bf___cam_joint is None:
T_bf___cam_joint = msg_to_kdl(lookup_transform('base_footprint', frame))
T_bf___cam_joint.p[2] = 0
T_bf___cam_joint.M = PyKDL.Rotation()
return T_bf___cam_joint
def check_cart_goal(self, tip, goal_pose):
goal_in_base = transform_pose(u'base_footprint', goal_pose)
current_pose = lookup_transform(u'base_footprint', tip)
np.testing.assert_array_almost_equal(
msg_to_list(goal_in_base.pose.position),
msg_to_list(current_pose.pose.position),
decimal=3)
np.testing.assert_array_almost_equal(
msg_to_list(goal_in_base.pose.orientation),
msg_to_list(current_pose.pose.orientation),
decimal=2)
def update(self):
"""
Computes closest point info for all robot links and safes it to the god map.
"""
with self.lock:
# TODO only update urdf if it has changed
self.god_map.set_data([self.robot_description_identifier],
self.world.get_robot().get_urdf())
js = self.god_map.get_data([self.js_identifier])
if js is not None:
self.world.set_robot_joint_state(js)
for object_name, object_joint_state in self.object_joint_states.items(
):
self.world.get_object(object_name).set_joint_state(
object_joint_state)
# TODO we can look up the transform outside of this loop
p = lookup_transform(self.map_frame, self.robot_root)
self.world.get_robot().set_base_pose(position=[
p.pose.position.x, p.pose.position.y, p.pose.position.z
],
orientation=[
p.pose.orientation.x,
p.pose.orientation.y,
p.pose.orientation.z,
p.pose.orientation.w
])
# TODO not necessary to parse collision goals every time
collision_goals = self.god_map.get_data(
[self.collision_goal_identifier])
collision_matrix = self.collision_goals_to_collision_matrix(
collision_goals)
collisions = self.world.check_collisions(
collision_matrix,
enable_self_collision=self.enable_self_collision)
closest_point = self.collisions_to_closest_point(
collisions, collision_matrix)
if self.marker:
self.publish_cpi_markers(closest_point)
self.god_map.set_data([self.closest_point_identifier],
closest_point)
from giskardpy.python_interface import GiskardWrapper
from giskardpy.tfwrapper import lookup_transform
if __name__ == '__main__':
rospy.init_node('add_urdf')
giskard = GiskardWrapper()
try:
name = rospy.get_param('~name')
path = rospy.get_param('~path', None)
param_name = rospy.get_param('~param', None)
position = rospy.get_param('~position', None)
orientation = rospy.get_param('~rpy', None)
root_frame = rospy.get_param('~root_frame', None)
map_frame = rospy.get_param('~frame_id', 'map')
if root_frame is not None:
pose = lookup_transform(map_frame, root_frame)
else:
pose = PoseStamped()
pose.header.frame_id = map_frame
if position is not None:
pose.pose.position = Point(*position)
if orientation is not None:
pose.pose.orientation = Quaternion(*quaternion_from_euler(
*orientation))
else:
pose.pose.orientation.w = 1
if path is None:
if param_name is None:
rospy.logwarn('neither _param nor _path specified')
sys.exit()
else: