class GiskardTestWrapper(object):
def __init__(self, config_file):
with open(get_ros_pkg_path(u'giskardpy') + u'/config/' +
config_file) as f:
config = yaml.load(f)
rospy.set_param('~', config)
rospy.set_param('~path_to_data_folder', u'tmp_data/')
rospy.set_param('~enable_gui', False)
rospy.set_param('~plugins/PlotTrajectory/enabled', True)
self.sub_result = rospy.Subscriber('~command/result',
MoveActionResult,
self.cb,
queue_size=100)
self.cancel_goal = rospy.Publisher('~command/cancel',
GoalID,
queue_size=100)
self.tree = grow_tree()
self.loop_once()
# rospy.sleep(1)
self.wrapper = GiskardWrapper(node_name=u'tests')
self.results = Queue(100)
self.default_root = self.get_robot().get_root()
self.map = u'map'
self.simple_base_pose_pub = rospy.Publisher('/move_base_simple/goal',
PoseStamped,
queue_size=10)
self.set_base = rospy.ServiceProxy('/base_simulator/set_joint_states',
SetJointState)
self.tick_rate = 10
def create_publisher(topic):
p = rospy.Publisher(topic, JointState, queue_size=10)
rospy.sleep(.2)
return p
self.joint_state_publisher = KeyDefaultDict(create_publisher)
# rospy.sleep(1)
def wait_for_synced(self):
sleeper = rospy.Rate(self.tick_rate)
self.loop_once()
# while self.tree.tip().name != u'has goal':
# self.loop_once()
# sleeper.sleep()
# self.loop_once()
# while self.tree.tip().name != u'has goal':
# self.loop_once()
# sleeper.sleep()
def get_robot(self):
"""
:rtype: Robot
"""
return self.get_god_map().get_data(robot)
def get_god_map(self):
"""
:rtype: giskardpy.god_map.GodMap
"""
return Blackboard().god_map
def cb(self, msg):
"""
:type msg: MoveActionResult
"""
self.results.put(msg.result)
def loop_once(self):
self.tree.tick()
def get_controlled_joint_names(self):
"""
:rtype: dict
"""
return self.get_robot().controlled_joints
def get_controllable_links(self):
return self.get_robot().get_controlled_links()
def get_current_joint_state(self):
"""
:rtype: JointState
"""
return rospy.wait_for_message('joint_states', JointState)
def tear_down(self):
rospy.sleep(1)
logging.loginfo(u'stopping plugins')
def set_object_joint_state(self, object_name, joint_state):
self.wrapper.set_object_joint_state(object_name, joint_state)
rospy.sleep(0.5)
self.wait_for_synced()
current_js = self.get_world().get_object(object_name).joint_state
for joint_name, state in joint_state.items():
np.testing.assert_almost_equal(current_js[joint_name].position,
state, 2)
def set_kitchen_js(self, joint_state, object_name=u'kitchen'):
self.set_object_joint_state(object_name, joint_state)
#
# JOINT GOAL STUFF #################################################################################################
#
def compare_joint_state(self, current_js, goal_js, decimal=2):
"""
:type current_js: dict
:type goal_js: dict
:type decimal: int
"""
joint_names = set(current_js.keys()).intersection(set(goal_js.keys()))
for joint_name in joint_names:
goal = goal_js[joint_name]
current = current_js[joint_name]
if self.get_robot().is_joint_continuous(joint_name):
np.testing.assert_almost_equal(shortest_angular_distance(
goal, current),
0,
decimal=decimal)
else:
np.testing.assert_almost_equal(
current,
goal,
decimal,
err_msg='{} at {} insteand of {}'.format(
joint_name, current, goal))
def set_joint_goal(self, js, weight=None):
"""
:rtype js: dict
"""
self.wrapper.set_joint_goal(js, weight=weight)
def check_joint_state(self, expected, decimal=2):
current_joint_state = to_joint_state_position_dict(
self.get_current_joint_state())
self.compare_joint_state(current_joint_state,
expected,
decimal=decimal)
def send_and_check_joint_goal(self,
goal,
weight=None,
decimal=2,
expected_error_codes=None):
"""
:type goal: dict
"""
self.set_joint_goal(goal, weight=weight)
self.send_and_check_goal(expected_error_codes=expected_error_codes)
if expected_error_codes == [MoveResult.SUCCESS]:
self.check_joint_state(goal, decimal=decimal)
#
# CART GOAL STUFF ##################################################################################################
#
def teleport_base(self, goal_pose):
goal_pose = transform_pose(self.default_root, goal_pose)
js = {
u'odom_x_joint':
goal_pose.pose.position.x,
u'odom_y_joint':
goal_pose.pose.position.y,
u'odom_z_joint':
rotation_from_matrix(
quaternion_matrix([
goal_pose.pose.orientation.x, goal_pose.pose.orientation.y,
goal_pose.pose.orientation.z, goal_pose.pose.orientation.w
]))[0]
}
goal = SetJointStateRequest()
goal.state = position_dict_to_joint_states(js)
self.set_base.call(goal)
self.loop_once()
rospy.sleep(0.5)
self.loop_once()
def keep_position(self, tip, root=None):
if root is None:
root = self.default_root
goal = PoseStamped()
goal.header.frame_id = tip
goal.pose.orientation.w = 1
self.set_cart_goal(goal, tip, root)
def keep_orientation(self, tip, root=None):
goal = PoseStamped()
goal.header.frame_id = tip
goal.pose.orientation.w = 1
self.set_rotation_goal(goal, tip, root)
def align_planes(self,
tip_link,
tip_normal,
root_link=None,
root_normal=None,
weight=None):
self.wrapper.align_planes(tip_link, tip_normal, root_link, root_normal,
weight)
def set_rotation_goal(self,
goal_pose,
tip_link,
root_link=None,
max_velocity=None):
if not root_link:
root_link = self.default_root
self.wrapper.set_rotation_goal(root_link,
tip_link,
goal_pose,
max_velocity=max_velocity)
def set_translation_goal(self,
goal_pose,
tip_link,
root_link=None,
max_velocity=None):
if not root_link:
root_link = self.default_root
self.wrapper.set_translation_goal(root_link,
tip_link,
goal_pose,
max_velocity=max_velocity)
def set_cart_goal(self,
goal_pose,
tip_link,
root_link=None,
weight=None,
linear_velocity=None,
angular_velocity=None):
if not root_link:
root_link = self.default_root
if weight is not None:
self.wrapper.set_cart_goal(root_link,
tip_link,
goal_pose,
weight=weight,
max_linear_velocity=linear_velocity,
max_angular_velocity=angular_velocity)
else:
self.wrapper.set_cart_goal(root_link,
tip_link,
goal_pose,
max_linear_velocity=linear_velocity,
max_angular_velocity=angular_velocity)
def set_and_check_cart_goal(self,
goal_pose,
tip_link,
root_link=None,
weight=None,
linear_velocity=None,
angular_velocity=None,
expected_error_codes=None):
goal_pose_in_map = transform_pose(u'map', goal_pose)
self.set_cart_goal(goal_pose,
tip_link,
root_link,
weight,
linear_velocity=linear_velocity,
angular_velocity=angular_velocity)
self.loop_once()
self.send_and_check_goal(expected_error_codes)
self.loop_once()
if expected_error_codes is None:
self.check_cart_goal(tip_link, goal_pose_in_map)
def check_cart_goal(self, tip_link, goal_pose):
goal_in_base = transform_pose(u'map', goal_pose)
current_pose = lookup_pose(u'map', tip_link)
np.testing.assert_array_almost_equal(
msg_to_list(goal_in_base.pose.position),
msg_to_list(current_pose.pose.position),
decimal=2)
try:
np.testing.assert_array_almost_equal(
msg_to_list(goal_in_base.pose.orientation),
msg_to_list(current_pose.pose.orientation),
decimal=2)
except AssertionError:
np.testing.assert_array_almost_equal(
msg_to_list(goal_in_base.pose.orientation),
-np.array(msg_to_list(current_pose.pose.orientation)),
decimal=2)
#
# GENERAL GOAL STUFF ###############################################################################################
#
def interrupt(self):
self.cancel_goal.publish(GoalID())
def check_reachability(self, expected_error_codes=None):
self.send_and_check_goal(
expected_error_codes=expected_error_codes,
goal_type=MoveGoal.PLAN_AND_CHECK_REACHABILITY)
def get_as(self):
return Blackboard().get(u'~command')
def send_goal(self,
goal=None,
goal_type=MoveGoal.PLAN_AND_EXECUTE,
wait=True):
"""
:rtype: MoveResult
"""
if goal is None:
goal = MoveActionGoal()
goal.goal = self.wrapper._get_goal()
goal.goal.type = goal_type
i = 0
self.loop_once()
t1 = Thread(
target=self.get_as()._as.action_server.internal_goal_callback,
args=(goal, ))
self.loop_once()
t1.start()
sleeper = rospy.Rate(self.tick_rate)
while self.results.empty():
self.loop_once()
sleeper.sleep()
i += 1
t1.join()
self.loop_once()
result = self.results.get()
return result
def send_goal_and_dont_wait(self,
goal=None,
goal_type=MoveGoal.PLAN_AND_EXECUTE,
stop_after=20):
if goal is None:
goal = MoveActionGoal()
goal.goal = self.wrapper._get_goal()
goal.goal.type = goal_type
i = 0
self.loop_once()
t1 = Thread(
target=self.get_as()._as.action_server.internal_goal_callback,
args=(goal, ))
self.loop_once()
t1.start()
sleeper = rospy.Rate(self.tick_rate)
while self.results.empty():
self.loop_once()
sleeper.sleep()
i += 1
if i > stop_after:
self.interrupt()
t1.join()
self.loop_once()
result = self.results.get()
return result
def send_and_check_goal(self,
expected_error_codes=None,
goal_type=MoveGoal.PLAN_AND_EXECUTE,
goal=None):
r = self.send_goal(goal=goal, goal_type=goal_type)
for i in range(len(r.error_codes)):
error_code = r.error_codes[i]
error_message = r.error_messages[i]
if expected_error_codes is None:
expected_error_code = MoveResult.SUCCESS
else:
expected_error_code = expected_error_codes[i]
assert error_code == expected_error_code, \
u'in goal {}; got: {}, expected: {} | error_massage: {}'.format(i, move_result_error_code(error_code),
move_result_error_code(
expected_error_code),
error_message)
return r.trajectory
def add_waypoint(self):
self.wrapper.add_cmd()
def add_json_goal(self, constraint_type, **kwargs):
self.wrapper.set_json_goal(constraint_type, **kwargs)
def get_result_trajectory_position(self):
trajectory = self.get_god_map().unsafe_get_data(identifier.trajectory)
trajectory2 = []
for t, p in trajectory._points.items():
trajectory2.append(
{joint_name: js.position
for joint_name, js in p.items()})
return trajectory2
def get_result_trajectory_velocity(self):
trajectory = self.get_god_map().get_data(identifier.trajectory)
trajectory2 = []
for t, p in trajectory._points.items():
trajectory2.append(
{joint_name: js.velocity
for joint_name, js in p.items()})
return trajectory2
def are_joint_limits_violated(self):
controllable_joints = self.get_robot().get_movable_joints()
trajectory_pos = self.get_result_trajectory_position()
trajectory_vel = self.get_result_trajectory_velocity()
for joint in controllable_joints:
joint_limits = self.get_robot().get_joint_limits(joint)
vel_limit = self.get_robot().get_joint_velocity_limit(joint)
trajectory_pos_joint = [p[joint] for p in trajectory_pos]
trajectory_vel_joint = [p[joint] for p in trajectory_vel]
if any(
round(p, 7) < joint_limits[0]
and round(p, 7) > joint_limits[1]
for p in trajectory_pos_joint):
return True
if any(
round(p, 7) < vel_limit and round(p, 7) > vel_limit
for p in trajectory_vel_joint):
return True
return False
#
# BULLET WORLD #####################################################################################################
#
def get_world(self):
"""
:rtype: PyBulletWorld
"""
return self.get_god_map().get_data(world)
def clear_world(self):
assert self.wrapper.clear_world(
).error_codes == UpdateWorldResponse.SUCCESS
assert len(self.get_world().get_object_names()) == 0
assert len(self.wrapper.get_object_names().object_names) == 0
# assert len(self.get_robot().get_attached_objects()) == 0
# assert self.get_world().has_object(u'plane')
def remove_object(self,
name,
expected_response=UpdateWorldResponse.SUCCESS):
r = self.wrapper.remove_object(name)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
assert not self.get_world().has_object(name)
assert not name in self.wrapper.get_object_names().object_names
def detach_object(self,
name,
expected_response=UpdateWorldResponse.SUCCESS):
if expected_response == UpdateWorldResponse.SUCCESS:
p = self.get_robot().get_fk_pose(self.get_robot().get_root(), name)
p = transform_pose(u'map', p)
assert name in self.wrapper.get_attached_objects().object_names, \
'there is no attached object named {}'.format(name)
r = self.wrapper.detach_object(name)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
if expected_response == UpdateWorldResponse.SUCCESS:
assert self.get_world().has_object(name)
assert not name in self.wrapper.get_attached_objects(
).object_names, 'the object was not detached'
compare_poses(self.get_world().get_object(name).base_pose,
p.pose,
decimal=2)
def add_box(self,
name=u'box',
size=(1, 1, 1),
pose=None,
expected_response=UpdateWorldResponse.SUCCESS):
r = self.wrapper.add_box(name, size, pose=pose)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
p = transform_pose(u'map', pose)
o_p = self.get_world().get_object(name).base_pose
assert self.get_world().has_object(name)
compare_poses(p.pose, o_p)
assert name in self.wrapper.get_object_names().object_names
compare_poses(o_p, self.wrapper.get_object_info(name).pose.pose)
def add_sphere(self, name=u'sphere', size=1, pose=None):
r = self.wrapper.add_sphere(name=name, size=size, pose=pose)
assert r.error_codes == UpdateWorldResponse.SUCCESS, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(UpdateWorldResponse.SUCCESS))
assert self.get_world().has_object(name)
assert name in self.wrapper.get_object_names().object_names
o_p = self.get_world().get_object(name).base_pose
compare_poses(o_p, self.wrapper.get_object_info(name).pose.pose)
def add_cylinder(self, name=u'cylinder', size=[1, 1], pose=None):
r = self.wrapper.add_cylinder(name=name,
height=size[0],
radius=size[1],
pose=pose)
assert r.error_codes == UpdateWorldResponse.SUCCESS, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(UpdateWorldResponse.SUCCESS))
assert self.get_world().has_object(name)
assert name in self.wrapper.get_object_names().object_names
def add_mesh(self,
name=u'cylinder',
path=u'',
pose=None,
expected_error=UpdateWorldResponse.SUCCESS):
r = self.wrapper.add_mesh(name=name, mesh=path, pose=pose)
assert r.error_codes == expected_error, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_error))
if expected_error == UpdateWorldResponse.SUCCESS:
assert self.get_world().has_object(name)
assert name in self.wrapper.get_object_names().object_names
o_p = self.get_world().get_object(name).base_pose
compare_poses(o_p, self.wrapper.get_object_info(name).pose.pose)
else:
assert not self.get_world().has_object(name)
assert name not in self.wrapper.get_object_names().object_names
def add_urdf(self, name, urdf, pose, js_topic=u'', set_js_topic=None):
r = self.wrapper.add_urdf(name,
urdf,
pose,
js_topic,
set_js_topic=set_js_topic)
assert r.error_codes == UpdateWorldResponse.SUCCESS, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(UpdateWorldResponse.SUCCESS))
assert self.get_world().has_object(name)
assert name in self.wrapper.get_object_names().object_names
def add_CollisionShape(self,
name,
path,
pose,
js_topic=u'',
set_js_topic=None):
r = self.wrapper.add_urdf(name,
urdf,
pose,
js_topic,
set_js_topic=set_js_topic)
assert r.error_codes == UpdateWorldResponse.SUCCESS, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(UpdateWorldResponse.SUCCESS))
assert self.get_world().has_object(name)
assert name in self.wrapper.get_object_names().object_names
def allow_all_collisions(self):
self.wrapper.allow_all_collisions()
def avoid_all_collisions(self, distance=0.5):
self.wrapper.avoid_all_collisions(distance)
self.loop_once()
def allow_self_collision(self):
self.wrapper.allow_self_collision()
def avoid_self_collision(self):
self.wrapper.avoid_self_collision()
def add_collision_entries(self, collisions_entries):
self.wrapper.set_collision_entries(collisions_entries)
def allow_collision(self, robot_links, body_b, link_bs):
ces = []
ces.append(
CollisionEntry(type=CollisionEntry.ALLOW_COLLISION,
robot_links=robot_links,
body_b=body_b,
link_bs=link_bs))
self.add_collision_entries(ces)
def avoid_collision(self, robot_links, body_b, link_bs, min_dist):
ces = []
ces.append(
CollisionEntry(type=CollisionEntry.AVOID_COLLISION,
robot_links=robot_links,
body_b=body_b,
link_bs=link_bs,
min_dist=min_dist))
self.add_collision_entries(ces)
def attach_box(self,
name=u'box',
size=None,
frame_id=None,
position=None,
orientation=None,
pose=None,
expected_response=UpdateWorldResponse.SUCCESS):
scm = self.get_robot().get_self_collision_matrix()
if pose is None:
expected_pose = PoseStamped()
expected_pose.header.frame_id = frame_id
expected_pose.pose.position = Point(*position)
if orientation:
expected_pose.pose.orientation = Quaternion(*orientation)
else:
expected_pose.pose.orientation = Quaternion(0, 0, 0, 1)
else:
expected_pose = deepcopy(pose)
r = self.wrapper.attach_box(name, size, frame_id, position,
orientation, pose)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
if expected_response == UpdateWorldResponse.SUCCESS:
self.wait_for_synced()
assert name in self.get_controllable_links()
assert not self.get_world().has_object(name)
assert not name in self.wrapper.get_object_names().object_names
assert name in self.wrapper.get_attached_objects(
).object_names, 'object {} was not attached'
assert scm.difference(
self.get_robot().get_self_collision_matrix()) == set()
assert len(scm) < len(self.get_robot().get_self_collision_matrix())
compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose)
self.loop_once()
def attach_cylinder(self,
name=u'cylinder',
height=1,
radius=1,
frame_id=None,
position=None,
orientation=None,
expected_response=UpdateWorldResponse.SUCCESS):
scm = self.get_robot().get_self_collision_matrix()
expected_pose = PoseStamped()
expected_pose.header.frame_id = frame_id
expected_pose.pose.position = Point(*position)
if orientation:
expected_pose.pose.orientation = Quaternion(*orientation)
else:
expected_pose.pose.orientation = Quaternion(0, 0, 0, 1)
r = self.wrapper.attach_cylinder(name, height, radius, frame_id,
position, orientation)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
if expected_response == UpdateWorldResponse.SUCCESS:
self.wait_for_synced()
assert name in self.get_controllable_links()
assert not self.get_world().has_object(name)
assert not name in self.wrapper.get_object_names().object_names
assert name in self.wrapper.get_attached_objects().object_names
assert scm.difference(
self.get_robot().get_self_collision_matrix()) == set()
assert len(scm) < len(self.get_robot().get_self_collision_matrix())
compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose)
self.loop_once()
def attach_existing(self,
name=u'box',
frame_id=None,
expected_response=UpdateWorldResponse.SUCCESS):
scm = self.get_robot().get_self_collision_matrix()
r = self.wrapper.attach_object(name, frame_id)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
self.wait_for_synced()
assert name in self.get_controllable_links()
assert not self.get_world().has_object(name)
assert not name in self.wrapper.get_object_names().object_names
assert name in self.wrapper.get_attached_objects().object_names
assert scm.difference(
self.get_robot().get_self_collision_matrix()) == set()
assert len(scm) < len(self.get_robot().get_self_collision_matrix())
self.loop_once()
def get_external_collisions(self, link, distance_threshold):
"""
:param distance_threshold:
:rtype: list
"""
collision_goals = [
CollisionEntry(type=CollisionEntry.AVOID_ALL_COLLISIONS,
min_dist=distance_threshold)
]
collision_matrix = self.get_world(
).collision_goals_to_collision_matrix(collision_goals,
defaultdict(lambda: 0.3))
collisions = self.get_world().check_collisions(collision_matrix)
controlled_parent_joint = self.get_robot().get_controlled_parent_joint(
link)
controlled_parent_link = self.get_robot().get_child_link_of_joint(
controlled_parent_joint)
collision_list = collisions.get_external_collisions(
controlled_parent_link)
for key, self_collisions in collisions.self_collisions.items():
if controlled_parent_link in key:
collision_list.update(self_collisions)
return collision_list
def check_cpi_geq(self, links, distance_threshold):
for link in links:
collisions = self.get_external_collisions(link, distance_threshold)
assert collisions[0].get_contact_distance() >= distance_threshold, \
u'distance for {}: {} >= {}'.format(link,
collisions[0].get_contact_distance(),
distance_threshold)
def check_cpi_leq(self, links, distance_threshold):
for link in links:
collisions = self.get_external_collisions(link, distance_threshold)
assert collisions[0].get_contact_distance() <= distance_threshold, \
u'distance for {}: {} <= {}'.format(link,
collisions[0].get_contact_distance(),
distance_threshold)
def move_base(self, goal_pose):
"""
:type goal_pose: PoseStamped
"""
self.simple_base_pose_pub.publish(goal_pose)
rospy.sleep(.07)
self.wait_for_synced()
current_pose = self.get_robot().get_base_pose()
goal_pose = transform_pose(u'map', goal_pose)
compare_poses(goal_pose.pose, current_pose.pose, decimal=1)
def reset_base(self):
p = PoseStamped()
p.header.frame_id = self.map
p.pose.orientation.w = 1
self.teleport_base(p)
class GiskardTestWrapper(object):
def __init__(self, config_file):
with open(get_ros_pkg_path(u'giskardpy') + u'/config/' +
config_file) as f:
config = yaml.load(f)
rospy.set_param(u'~', config)
rospy.set_param(u'~path_to_data_folder', u'tmp_data/')
rospy.set_param(u'~enable_gui', False)
self.sub_result = rospy.Subscriber(u'/giskardpy/command/result',
MoveActionResult,
self.cb,
queue_size=100)
self.tree = grow_tree()
self.loop_once()
# rospy.sleep(1)
self.wrapper = GiskardWrapper(ns=u'tests')
self.results = Queue(100)
self.default_root = self.get_robot().get_root()
self.map = u'map'
self.simple_base_pose_pub = rospy.Publisher(u'/move_base_simple/goal',
PoseStamped,
queue_size=10)
self.set_base = rospy.ServiceProxy(u'/base_simulator/set_joint_states',
SetJointState)
self.tick_rate = 10
def create_publisher(topic):
p = rospy.Publisher(topic, JointState, queue_size=10)
rospy.sleep(.2)
return p
self.joint_state_publisher = KeyDefaultDict(create_publisher)
# rospy.sleep(1)
def wait_for_synced(self):
sleeper = rospy.Rate(self.tick_rate)
self.loop_once()
# while self.tree.tip().name != u'has goal':
# self.loop_once()
# sleeper.sleep()
# self.loop_once()
# while self.tree.tip().name != u'has goal':
# self.loop_once()
# sleeper.sleep()
def get_robot(self):
"""
:rtype: Robot
"""
return self.get_god_map().safe_get_data(robot)
def get_god_map(self):
"""
:rtype: giskardpy.god_map.GodMap
"""
return Blackboard().god_map
def cb(self, msg):
"""
:type msg: MoveActionResult
"""
self.results.put(msg.result)
def loop_once(self):
self.tree.tick()
def get_controlled_joint_names(self):
"""
:rtype: dict
"""
return self.get_robot().controlled_joints
def get_controllable_links(self):
return self.get_robot().get_controlled_links()
def get_current_joint_state(self):
"""
:rtype: JointState
"""
return rospy.wait_for_message(u'joint_states', JointState)
def tear_down(self):
rospy.sleep(1)
logging.loginfo(u'stopping plugins')
def set_object_joint_state(self, object_name, joint_state, topic=None):
if topic is None:
self.wrapper.set_object_joint_state(object_name, joint_state)
else:
self.joint_state_publisher[topic].publish(
dict_to_joint_states(joint_state))
rospy.sleep(.5)
self.wait_for_synced()
current_js = self.get_world().get_object(object_name).joint_state
for joint_name, state in joint_state.items():
np.testing.assert_almost_equal(current_js[joint_name].position,
state, 2)
def set_kitchen_js(self, joint_state, object_name=u'kitchen'):
self.set_object_joint_state(object_name,
joint_state,
topic=u'/kitchen/cram_joint_states')
#
# JOINT GOAL STUFF #################################################################################################
#
def compare_joint_state(self, current_js, goal_js, decimal=2):
"""
:type current_js: dict
:type goal_js: dict
:type decimal: int
"""
joint_names = set(current_js.keys()).intersection(set(goal_js.keys()))
for joint_name in joint_names:
goal = goal_js[joint_name]
current = current_js[joint_name]
if self.get_robot().is_joint_continuous(joint_name):
np.testing.assert_almost_equal(shortest_angular_distance(
goal, current),
0,
decimal=decimal)
else:
np.testing.assert_almost_equal(
current,
goal,
decimal,
err_msg=u'{} at {} insteand of {}'.format(
joint_name, current, goal))
def set_joint_goal(self, js):
"""
:rtype js: dict
"""
self.wrapper.set_joint_goal(js)
def check_joint_state(self, expected, decimal=2):
current_joint_state = to_joint_state_dict2(
self.get_current_joint_state())
self.compare_joint_state(current_joint_state,
expected,
decimal=decimal)
def send_and_check_joint_goal(self, goal, decimal=2):
"""
:type goal: dict
"""
self.set_joint_goal(goal)
self.send_and_check_goal()
self.check_joint_state(goal, decimal=decimal)
#
# CART GOAL STUFF ##################################################################################################
#
def teleport_base(self, goal_pose):
goal_pose = transform_pose(self.default_root, goal_pose)
js = {
u'odom_x_joint':
goal_pose.pose.position.x,
u'odom_y_joint':
goal_pose.pose.position.y,
u'odom_z_joint':
rotation_from_matrix(
quaternion_matrix([
goal_pose.pose.orientation.x, goal_pose.pose.orientation.y,
goal_pose.pose.orientation.z, goal_pose.pose.orientation.w
]))[0]
}
goal = SetJointStateRequest()
goal.state = dict_to_joint_states(js)
self.set_base.call(goal)
def keep_position(self, tip, root=None):
if root is None:
root = self.default_root
goal = PoseStamped()
goal.header.frame_id = tip
goal.pose.orientation.w = 1
self.set_cart_goal(goal, tip, root)
def keep_orientation(self, tip, root=None):
goal = PoseStamped()
goal.header.frame_id = tip
goal.pose.orientation.w = 1
self.set_rotation_goal(goal, tip, root)
def align_planes(self, tip, tip_normal, root=None, root_normal=None):
self.wrapper.align_planes(tip, tip_normal, root, root_normal)
def set_rotation_goal(self, goal_pose, tip, root=None):
if not root:
root = self.default_root
self.wrapper.set_rotation_goal(root, tip, goal_pose)
def set_translation_goal(self, goal_pose, tip, root=None):
if not root:
root = self.default_root
self.wrapper.set_translation_goal(root, tip, goal_pose)
def set_cart_goal(self, goal_pose, tip, root=None):
if not root:
root = self.default_root
self.wrapper.set_cart_goal(root, tip, goal_pose)
def set_and_check_cart_goal(self,
goal_pose,
tip,
root=None,
expected_error_code=MoveResult.SUCCESS):
goal_pose_in_map = transform_pose(u'map', goal_pose)
self.set_cart_goal(goal_pose, tip, root)
self.loop_once()
self.send_and_check_goal(expected_error_code)
self.loop_once()
self.check_cart_goal(tip, goal_pose_in_map)
def check_cart_goal(self, tip, goal_pose):
goal_in_base = transform_pose(u'map', goal_pose)
current_pose = lookup_pose(u'map', tip)
np.testing.assert_array_almost_equal(
msg_to_list(goal_in_base.pose.position),
msg_to_list(current_pose.pose.position),
decimal=2)
try:
np.testing.assert_array_almost_equal(
msg_to_list(goal_in_base.pose.orientation),
msg_to_list(current_pose.pose.orientation),
decimal=2)
except AssertionError:
np.testing.assert_array_almost_equal(
msg_to_list(goal_in_base.pose.orientation),
-np.array(msg_to_list(current_pose.pose.orientation)),
decimal=2)
#
# GENERAL GOAL STUFF ###############################################################################################
#
def get_as(self):
return Blackboard().get(u'giskardpy/command')
def send_goal(self, goal=None, execute=True):
"""
:rtype: MoveResult
"""
if goal is None:
goal = MoveActionGoal()
goal.goal = self.wrapper._get_goal()
if execute:
goal.goal.type = MoveGoal.PLAN_AND_EXECUTE
else:
goal.goal.type = MoveGoal.PLAN_ONLY
i = 0
self.loop_once()
t1 = Thread(
target=self.get_as()._as.action_server.internal_goal_callback,
args=(goal, ))
self.loop_once()
t1.start()
sleeper = rospy.Rate(self.tick_rate)
while self.results.empty():
self.loop_once()
sleeper.sleep()
i += 1
t1.join()
self.loop_once()
result = self.results.get()
return result
def send_and_check_goal(self,
expected_error_code=MoveResult.SUCCESS,
execute=True):
r = self.send_goal(execute=execute)
assert r.error_code == expected_error_code, \
u'got: {}, expected: {}'.format(move_result_error_code(r.error_code),
move_result_error_code(expected_error_code))
def add_waypoint(self):
self.wrapper.add_cmd()
def add_json_goal(self, constraint_type, **kwargs):
self.wrapper.set_json_goal(constraint_type, **kwargs)
def get_trajectory_msg(self):
trajectory = self.get_god_map().get_data(identifier.trajectory)
trajectory2 = []
for t, p in trajectory._points.items():
trajectory2.append(
{joint_name: js.position
for joint_name, js in p.items()})
return trajectory2
#
# BULLET WORLD #####################################################################################################
#
def get_world(self):
"""
:rtype: PyBulletWorld
"""
return self.get_god_map().safe_get_data(world)
def clear_world(self):
assert self.wrapper.clear_world(
).error_codes == UpdateWorldResponse.SUCCESS
assert len(self.get_world().get_object_names()) == 0
# assert len(self.get_robot().get_attached_objects()) == 0
# assert self.get_world().has_object(u'plane')
def remove_object(self,
name,
expected_response=UpdateWorldResponse.SUCCESS):
r = self.wrapper.remove_object(name)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
assert not self.get_world().has_object(name)
def detach_object(self,
name,
expected_response=UpdateWorldResponse.SUCCESS):
if expected_response == UpdateWorldResponse.SUCCESS:
p = self.get_robot().get_fk_pose(self.get_robot().get_root(), name)
p = transform_pose(u'map', p)
r = self.wrapper.detach_object(name)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
if expected_response == UpdateWorldResponse.SUCCESS:
assert self.get_world().has_object(name)
compare_poses(self.get_world().get_object(name).base_pose,
p.pose,
decimal=2)
def add_box(self,
name=u'box',
size=(1, 1, 1),
pose=None,
expected_response=UpdateWorldResponse.SUCCESS):
r = self.wrapper.add_box(name, size, pose=pose)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
p = transform_pose(u'map', pose)
o_p = self.get_world().get_object(name).base_pose
assert self.get_world().has_object(name)
compare_poses(p.pose, o_p)
def add_sphere(self, name=u'sphere', size=1, pose=None):
r = self.wrapper.add_sphere(name=name, size=size, pose=pose)
assert r.error_codes == UpdateWorldResponse.SUCCESS, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(UpdateWorldResponse.SUCCESS))
assert self.get_world().has_object(name)
def add_cylinder(self, name=u'cylinder', size=[1, 1], pose=None):
r = self.wrapper.add_cylinder(name=name, size=size, pose=pose)
assert r.error_codes == UpdateWorldResponse.SUCCESS, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(UpdateWorldResponse.SUCCESS))
assert self.get_world().has_object(name)
def add_urdf(self, name, urdf, pose, js_topic):
r = self.wrapper.add_urdf(name, urdf, pose, js_topic)
assert r.error_codes == UpdateWorldResponse.SUCCESS, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(UpdateWorldResponse.SUCCESS))
assert self.get_world().has_object(name)
def allow_all_collisions(self):
self.wrapper.allow_all_collisions()
def avoid_all_collisions(self, distance=0.5):
self.wrapper.avoid_all_collisions(distance)
self.loop_once()
def enable_self_collision(self):
pass
def allow_self_collision(self):
self.wrapper.allow_self_collision()
def add_collision_entries(self, collisions_entries):
self.wrapper.set_collision_entries(collisions_entries)
def allow_collision(self, robot_links, body_b, link_bs):
ces = []
ces.append(
CollisionEntry(type=CollisionEntry.ALLOW_COLLISION,
robot_links=robot_links,
body_b=body_b,
link_bs=link_bs))
self.add_collision_entries(ces)
def avoid_collision(self, robot_links, body_b, link_bs, min_dist):
ces = []
ces.append(
CollisionEntry(type=CollisionEntry.AVOID_COLLISION,
robot_links=robot_links,
body_b=body_b,
link_bs=link_bs,
min_dist=min_dist))
self.add_collision_entries(ces)
def attach_box(self,
name=u'box',
size=None,
frame_id=None,
position=None,
orientation=None,
expected_response=UpdateWorldResponse.SUCCESS):
scm = self.get_robot().get_self_collision_matrix()
expected_pose = PoseStamped()
expected_pose.header.frame_id = frame_id
expected_pose.pose.position = Point(*position)
if orientation:
expected_pose.pose.orientation = Quaternion(*orientation)
else:
expected_pose.pose.orientation = Quaternion(0, 0, 0, 1)
r = self.wrapper.attach_box(name, size, frame_id, position,
orientation)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
if expected_response == UpdateWorldResponse.SUCCESS:
self.wait_for_synced()
assert name in self.get_controllable_links()
assert not self.get_world().has_object(name)
assert scm.difference(
self.get_robot().get_self_collision_matrix()) == set()
assert len(scm) < len(self.get_robot().get_self_collision_matrix())
compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose)
self.loop_once()
def attach_cylinder(self,
name=u'cylinder',
height=1,
radius=1,
frame_id=None,
position=None,
orientation=None,
expected_response=UpdateWorldResponse.SUCCESS):
scm = self.get_robot().get_self_collision_matrix()
expected_pose = PoseStamped()
expected_pose.header.frame_id = frame_id
expected_pose.pose.position = Point(*position)
if orientation:
expected_pose.pose.orientation = Quaternion(*orientation)
else:
expected_pose.pose.orientation = Quaternion(0, 0, 0, 1)
r = self.wrapper.attach_cylinder(name, height, radius, frame_id,
position, orientation)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
if expected_response == UpdateWorldResponse.SUCCESS:
self.wait_for_synced()
assert name in self.get_controllable_links()
assert not self.get_world().has_object(name)
assert scm.difference(
self.get_robot().get_self_collision_matrix()) == set()
assert len(scm) < len(self.get_robot().get_self_collision_matrix())
compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose)
self.loop_once()
def attach_existing(self,
name=u'box',
frame_id=None,
expected_response=UpdateWorldResponse.SUCCESS):
scm = self.get_robot().get_self_collision_matrix()
r = self.wrapper.attach_object(name, frame_id)
assert r.error_codes == expected_response, \
u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
update_world_error_code(expected_response))
self.wait_for_synced()
assert name in self.get_controllable_links()
assert not self.get_world().has_object(name)
assert scm.difference(
self.get_robot().get_self_collision_matrix()) == set()
assert len(scm) < len(self.get_robot().get_self_collision_matrix())
self.loop_once()
def get_external_collisions(self, link, distance_threshold):
"""
:param distance_threshold:
:rtype: list
"""
collision_goals = [
CollisionEntry(type=CollisionEntry.AVOID_ALL_COLLISIONS,
min_dist=distance_threshold)
]
collision_matrix = self.get_world(
).collision_goals_to_collision_matrix(
collision_goals,
self.get_god_map().safe_get_data(identifier.distance_thresholds))
collisions = self.get_world().check_collisions(collision_matrix)
collisions = self.get_world().transform_contact_info(collisions)
collision_list = collisions.external_collision[
self.get_robot().get_movable_parent_joint(link)]
for key, self_collisions in collisions.self_collisions.items():
if link in key:
collision_list.update(self_collisions)
return collision_list
def check_cpi_geq(self, links, distance_threshold):
for link in links:
collisions = self.get_external_collisions(link, distance_threshold)
assert collisions[0].contact_distance >= distance_threshold
def check_cpi_leq(self, links, distance_threshold):
for link in links:
collisions = self.get_external_collisions(link, distance_threshold)
assert collisions[0].contact_distance <= distance_threshold
def move_base(self, goal_pose):
"""
:type goal_pose: PoseStamped
"""
self.simple_base_pose_pub.publish(goal_pose)
rospy.sleep(.07)
self.wait_for_synced()
current_pose = self.get_robot().get_base_pose()
goal_pose = transform_pose(u'map', goal_pose)
compare_poses(goal_pose.pose, current_pose.pose, decimal=1)
def reset_base(self):
p = PoseStamped()
p.header.frame_id = self.map
p.pose.orientation.w = 1
self.teleport_base(p)
class MotionTaskWithConstraint:
"""
the class helps to select the necessary constraints, to select the gripper and to check the orientation of the base.
"""
# list of Gripper
_list_grippers = []
# Torso of robot
_torso = ""
# Reference frame to basefootprint
_origin = ""
# goal frame
_goal = ""
def __init__(self):
print("MotionTaskWithConstraint is initialized !")
self._giskard = GiskardWrapper()
rospy.logout("- Set pose kitchen")
kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world")
kitchen_pose.header.frame_id = "map"
# setup kitchen
rospy.logout("- Get urdf")
self.urdf = rospy.get_param("kitchen_description")
self.parsed_urdf = URDF.from_xml_string(self.urdf)
self.config_file = None
rospy.logout("- clear urdf and add kitchen urdf")
self._giskard.clear_world()
self._giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states")
def set_gripper_for_real_roboter(self):
# setup gripper as actionlib
self.rgripper = actionlib.SimpleActionClient('r_gripper/gripper_command', GripperCommandAction)
self.lgripper = actionlib.SimpleActionClient('l_gripper/gripper_command', GripperCommandAction)
self.rgripper.wait_for_server()
self.lgripper.wait_for_server()
def r_action_gripper(self, value=0):
goal = Pr2GripperCommandGoal() #GripperCommandGoal()
goal.command.position = value
goal.command.max_effort = 40
self.rgripper.send_goal_and_wait(goal)
def l_action_gripper(self, value=0):
goal = Pr2GripperCommandGoal() #GripperCommandGoal()
goal.command.position = value
goal.command.max_effort = 40
self.lgripper.send_goal_and_wait(goal)
def execute_open_translational_motion_on_real_robot(self, goal, body, action, joint, joint_value):
self.r_action_gripper(0.08)
self.l_action_gripper(0.08)
self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
self._giskard.allow_all_collisions()
self._giskard.plan_and_execute()
self.r_action_gripper(0.012)
self.l_action_gripper(0.012)
self._giskard.set_json_goal("OpenCloseDrawerConstraint", goal_name=goal, body_name=body, action=action)
self._giskard.allow_all_collisions()
# self.giskard.avoid_all_collisions()
self._giskard.plan_and_execute()
self._giskard.set_object_joint_state(object_name='kitchen',
joint_states={joint: joint_value})
def execute_close_translational_motion_on_real_robot(self, goal, body, action, joint, joint_value):
self.r_action_gripper(0.08)
self.l_action_gripper(0.08)
self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
self._giskard.allow_all_collisions()
self._giskard.plan_and_execute()
self._giskard.set_json_goal("OpenCloseDrawerConstraint", goal_name=goal, body_name=body, action=action)
self._giskard.allow_all_collisions()
# self.giskard.avoid_all_collisions()
self._giskard.plan_and_execute()
self._giskard.set_object_joint_state(object_name='kitchen',
joint_states={joint: joint_value})
def execute_open_circular_motion_on_real_robot(self, goal, body, action, joint, joint_value):
self.r_action_gripper(0.08)
self.l_action_gripper(0.08)
self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
self._giskard.allow_all_collisions()
self._giskard.plan_and_execute()
self.r_action_gripper(0.012)
self.l_action_gripper(0.012)
self._giskard.set_json_goal("OpenCloseDoorConstraint", goal_name=goal, body_name=body, action=action)
self._giskard.allow_all_collisions()
# self.giskard.avoid_all_collisions()
self._giskard.plan_and_execute()
self._giskard.set_object_joint_state(object_name='kitchen',
joint_states={joint: joint_value})
def execute_close_circular_motion_on_real_robot(self, goal, body, action, joint, joint_value):
self.r_action_gripper(0.08)
self.l_action_gripper(0.08)
self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
self._giskard.allow_all_collisions()
self._giskard.plan_and_execute()
self._giskard.set_json_goal("OpenCloseDoorConstraint", goal_name=goal, body_name=body, action=action)
self._giskard.allow_all_collisions()
# self.giskard.avoid_all_collisions()
self._giskard.plan_and_execute()
self._giskard.set_object_joint_state(object_name='kitchen',
joint_states={joint: joint_value})
def execute_open_rotational_motion_on_real_robot(self, goal, body, action, joint, joint_value):
self.r_action_gripper(0.08)
self.l_action_gripper(0.08)
self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
self._giskard.allow_all_collisions()
self._giskard.plan_and_execute()
self.r_action_gripper(0.012)
self.l_action_gripper(0.012)
self._giskard.set_json_goal("TurnRotaryKnobConstraint", goal_name=goal, body_name=body, action=action)
self._giskard.allow_all_collisions()
# self.giskard.avoid_all_collisions()
self._giskard.plan_and_execute()
self._giskard.set_object_joint_state(object_name='kitchen',
joint_states={joint: joint_value})
def execute_close_rotational_motion_on_real_robot(self, goal, body, action, joint, joint_value):
self.r_action_gripper(0.08)
self.l_action_gripper(0.08)
self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
self._giskard.allow_all_collisions()
self._giskard.plan_and_execute()
self._giskard.set_json_goal("TurnRotaryKnobConstraint", goal_name=goal, body_name=body, action=action)
self.giskard.allow_all_collisions()
# self.giskard.avoid_all_collisions()
self.giskard.plan_and_execute()
self._giskard.set_object_joint_state(object_name='kitchen',
joint_states={joint: joint_value})
def setup_orientation_basis(self, basis_frame="odom_combined", torso="torso_lift_link",
list_grippers=["r_gripper_tool_frame", "l_gripper_tool_frame"]):
"""
This method set the parameter for the orientations verification of the basis
:param basis_frame: origin frame to the basis
:type: str
:param torso: torso of robot
:type: str
:param list_grippers: List of gripper
:type: array of str
:return:
"""
self._list_grippers = list_grippers
self._torso = torso
self._origin = basis_frame
def set_goal(self, goal=""):
"""
set the goal in relation to which the base is to be oriented
:param goal: goal
:type: str
:return:
"""
self._goal = goal
def update_basis_orientation(self):
"""
This function update the orientation of the basis and get the next Gripper in short distance from goal
:return:
"""
x, y, r = self.get_current_base_position()
rospy.logout("The basis is at position :")
rospy.logout(x)
rospy.logout(y)
rospy.logout(r)
# get pose torso
pose_torso = tf_wrapper.lookup_pose(self._origin, self._torso)
# get pose goal
pose_goal = tf_wrapper.lookup_pose(self._origin, self._goal)
# get list of pose of gripper
list_pose_gripper = [tf_wrapper.lookup_pose(self._origin, p) for p in self._list_grippers]
# get angle to G
angle = [float(w.get_angle_casadi(self.get_x_y(pose_torso), self.get_x_y(pose_goal), self.get_x_y(p)))
for p in list_pose_gripper]
# max angle G to goal
max_angle = np.amax(angle)
rospy.logout("Max angle is :")
rospy.logout(max_angle)
# update old and new orientation
max_angle = r + max_angle
rospy.logout("Max angle + r is :")
rospy.logout(max_angle)
# get selected Gripper
gripper = np.where(np.array(angle) == np.amax(angle))
rospy.logout("Farther away Gripper:")
rospy.logout(gripper[0][0])
# convert angle in axis
q = w.quaternion_from_axis_angle([0, 0, 1], max_angle)
# update basis orientation
#self.set_cart_goal_for_basis(x, y, 0, q)
#@staticmethod
def get_x_y(self, pose):
return [pose.pose.position.x, pose.pose.position.y, 0]
def get_current_base_position(self):
"""
the method check the current pose of base_footprint to odom and get it
:return: x, y, rotation
"""
basis = tf_wrapper.lookup_pose(self._origin, "base_footprint")
t, r = [basis.pose.position.x, basis.pose.position.y, basis.pose.position.z], \
[basis.pose.orientation.x, basis.pose.orientation.y, basis.pose.orientation.z,
basis.pose.orientation.w] # self.get_msg_translation_and_rotation(self._basis, "map")
return self.base_position(t, r)
def base_position(self, translation, rotation):
"""
the method take the translation and rotation from tf_ros and calculate the rotation in euler
:param translation: vector
:param rotation: quaternion (from tf_ros)
:return: vector[x, y, rotation], current base position
"""
euler = tf.transformations.euler_from_quaternion(rotation)
return translation[0], translation[1], euler[2]
def set_cart_goal_for_basis(self, x, y, z, q):
"""
this function send goal to the basis of robot
:param x: position on x axis
:type: float
:param y: position on y axis
:type: float
:param z: position on z axis
:type: float
:param q: quaternion orientation
:type: array float
:return:
"""
# take base_footprint
poseStamp = lookup_pose(self._origin, "base_footprint")
poseStamp.header.frame_id = self._origin
# set goal pose
poseStamp.pose.position.x = x
poseStamp.pose.position.y = y
poseStamp.pose.position.z = z
poseStamp.pose.orientation.x = q[0]
poseStamp.pose.orientation.y = q[1]
poseStamp.pose.orientation.z = q[2]
poseStamp.pose.orientation.w = q[3]
self._giskard.set_cart_goal(self._origin, "base_footprint", poseStamp)
self._giskard.avoid_all_collisions()
self._giskard.plan_and_execute()
def reset_kitchen(self, list_joints):
for j in list_joints:
self._giskard.set_object_joint_state(object_name='kitchen',
joint_states={j: 0.0})
class Gripper:
def __init__(self):
"""
this class controls and performs mvt of gripper
"""
self.giskard = GiskardWrapper()
rospy.logout("--> Set kitchen/world in Giskard")
rospy.logout("- Set pose kitchen")
kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world")
kitchen_pose.header.frame_id = "map"
# setup kitchen
rospy.logout("- Get urdf")
self.urdf = rospy.get_param("kitchen_description")
self.parsed_urdf = URDF.from_xml_string(self.urdf)
self.config_file = None
rospy.logout("- clear urdf and add kitchen urdf")
self.giskard.clear_world()
self.giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states")
# setup gripper as service
rospy.logout("- Set right and left Gripper service proxy")
self.l_gripper_service = rospy.ServiceProxy('/l_gripper_simulator/set_joint_states', SetJointState)
self.r_gripper_service = rospy.ServiceProxy('/r_gripper_simulator/set_joint_states', SetJointState)
# setup gripper as actionlib
# self.rgripper = actionlib.SimpleActionClient('r_gripper/gripper_command', GripperCommandAction)
# self.lgripper = actionlib.SimpleActionClient('l_gripper/gripper_command', GripperCommandAction)
# self.rgripper.wait_for_server()
# self.lgripper.wait_for_server()
rospy.logout("--> Gripper are ready for every task.")
def r_action_gripper(self, value=0):
goal = GripperCommandGoal()
goal.command.position = value
goal.command.max_effort = 10
self.rgripper.send_goal_and_wait(goal)
def l_action_gripper(self, value=0):
goal = GripperCommandGoal()
goal.command.position = value
goal.command.max_effort = 10
self.lgripper.send_goal_and_wait(goal)
def some_mvt(self, goal, body):
"""
execute of simple movement of position and rotation with the grippers
:param goal: goal name
:type: str
:param body: End Link, l_gripper or r_gripper
:type: str
:param axis: the axis on which the object must be caught, maybe 'x', 'y' or 'z'
:type: str
:return:
"""
# self.giskard.set_json_goal("MoveWithConstraint", goal_name=goal, body_name=body)
self.giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body) # movetopose
# self.giskard.set_json_goal("CartesianPoseUpdate", root_link="odom_combined", tip_link=body, goal_name=goal)
# self.giskard.avoid_all_collisions()
self.giskard.allow_all_collisions()
self.giskard.plan_and_execute()
def open_or_close_with_translation(self, goal, body, action):
"""
execute simple translation of gripper with something
:param goal: goal name (Frame)
:type: str
:param body: End Link, l_gripper or r_gripper
:type: str
:param action: percentage of movement
:type: float
:return:
"""
# self.giskard.set_json_goal("TranslationalAngularConstraint", goal_name=goal, body_name=body, action=action)
self.giskard.set_json_goal("OpenCloseDrawerConstraint", goal_name=goal, body_name=body, action=action) # openclosedrawerconstraint
#self.giskard.set_json_goal("OpenCloseDoorConstraint", goal_name=goal, body_name=body, action=action) # openclosedoorconstraint
self.giskard.allow_all_collisions()
# self.giskard.avoid_all_collisions()
self.giskard.plan_and_execute()
def do_rotational_mvt(self, goal, body, action):
"""
Execute rotational movement with the gripper(body)
:param goal: goal name
:type: str
:param body: End Link, l/r Gripper
:str: str
:param action: percentage of movement
:rtype: float
:return:
"""
self.giskard.set_json_goal("RotationalConstraint", goal_name=goal, body_name=body, action=action)
# self.giskard.allow_collision(robot_links=(body), link_bs=(goal))
self.giskard.allow_all_collisions()
self.giskard.plan_and_execute()
def mvt_l_gripper(self, l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint):
"""
execute mvt of left gripper
:param l_finger_joint: value of l_gripper_l_finger_joint
:type: float
:param r_finger_joint: value of l_gripper_r_finger_joint
:type: float
:param l_finger_tip_joint: value of l_gripper_l_finger_tip_joint
:type: float
:param r_finger_tip_joint: value of l_gripper_r_finger_tip_joint
:type: float
:return:
"""
rospy.logout('Start mvt L gripper')
gripper_request = SetJointStateRequest()
gripper_request.state.name = ['l_gripper_l_finger_joint', 'l_gripper_r_finger_joint',
'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_tip_joint']
gripper_request.state.position = [l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint]
gripper_request.state.velocity = [0, 0, 0, 0]
gripper_request.state.effort = [0, 0, 0, 0]
self.l_gripper_service.call(gripper_request)
rospy.logout('End mvt L gripper')
def mvt_r_gripper(self, l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint):
"""
execute mvt of right gripper
:param l_finger_joint: value of r_gripper_l_finger_joint
:type: float
:param r_finger_joint: value of r_gripper_r_finger_joint
:type: float
:param l_finger_tip_joint: value of r_gripper_l_finger_tip_joint
:type: float
:param r_finger_tip_joint: value of r_gripper_r_finger_tip_joint
:type: float
:return:
"""
rospy.logout('Start mvt R gripper')
gripper_request = SetJointStateRequest()
gripper_request.state.name = ['r_gripper_l_finger_joint', 'r_gripper_r_finger_joint',
'r_gripper_l_finger_tip_joint', 'r_gripper_r_finger_tip_joint']
gripper_request.state.position = [l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint]
gripper_request.state.velocity = [0, 0, 0, 0]
gripper_request.state.effort = [0, 0, 0, 0]
self.r_gripper_service.call(gripper_request)
rospy.logout('End mvt R gripper')
def get_pose(self, source, frame_id):
"""
the method get exactly the pose of link frame_id to source(map or odom)
:param source: odom or map, typ string
:param frame_id: string
:return: PoseStamped
"""
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
# print tf_listener
ps = PoseStamped()
transform = tf_buffer.lookup_transform(source,
frame_id,
rospy.Time(0),
rospy.Duration(5.0))
# print transform
return tf2_geometry_msgs.do_transform_pose(ps, transform)
def info_about_joints(self, joint_name):
"""
This method get info about some joint ( limits and axis)
:param joint_name: name of the joint
:type: String
:return: joint_limit, joint_axis
:rtype: {'effort': float, 'lower': float, 'upper': float, 'velocity': float}, array 1*3
"""
joint_limits = {}
joint_axis = None
for j in self.parsed_urdf.joints:
if joint_name == j.name:
if j.axis != None:
joint_axis = j.axis
if j.limit != None:
joint_limits = {'effort': j.limit.effort, 'lower': j.limit.lower,
'upper': j.limit.upper, 'velocity': j.limit.velocity}
return joint_limits, joint_axis
def set_kitchen_goal(self, joint_name, joint_value):
self.giskard.set_object_joint_state(object_name='kitchen',
joint_states={joint_name: joint_value})