def attach_object(self, req):
"""
:type req: UpdateWorldRequest
"""
if self.get_world().has_object(req.body.name):
p = PoseStamped()
p.header.frame_id = self.map_frame
p.pose = self.get_world().get_object(req.body.name).base_pose
p = transform_pose(req.pose.header.frame_id, p)
world_object = self.get_world().get_object(req.body.name)
self.get_world().attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose)
m = world_object.as_marker_msg()
m.header.frame_id = p.header.frame_id
m.pose = p.pose
else:
world_object = WorldObject.from_world_body(req.body)
self.get_world().robot.attach_urdf_object(world_object,
req.pose.header.frame_id,
req.pose.pose)
m = world_object.as_marker_msg()
m.pose = req.pose.pose
m.header = req.pose.header
try:
m.frame_locked = True
self.publish_object_as_marker(m)
except:
pass
def move_other_frame(self, target_pose, frame='camera_link', retry=True):
if self.enabled:
target_pose = self.cam_pose_to_base_pose(target_pose, frame)
target_pose = transform_pose('map', target_pose)
target_pose.pose.position.z = 0
self.giskard.set_cart_goal('odom', 'base_footprint', target_pose)
return self.giskard.plan_and_execute()
def attach_object(self, req):
"""
:type req: UpdateWorldRequest
"""
# assumes that parent has god map lock
if self.unsafe_get_world().has_object(req.body.name):
p = PoseStamped()
p.header.frame_id = self.map_frame
p.pose = self.unsafe_get_world().get_object(req.body.name).base_pose
p = transform_pose(req.pose.header.frame_id, p)
world_object = self.unsafe_get_world().get_object(req.body.name)
self.unsafe_get_world().attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose)
m = world_object.as_marker_msg()
m.header.frame_id = p.header.frame_id
m.pose = p.pose
else:
world_object = WorldObject.from_world_body(req.body)
self.unsafe_get_world().robot.attach_urdf_object(world_object,
req.pose.header.frame_id,
req.pose.pose)
logging.loginfo(u'--> attached object {} on link {}'.format(req.body.name, req.pose.header.frame_id))
m = world_object.as_marker_msg()
m.pose = req.pose.pose
m.header = req.pose.header
try:
m.frame_locked = True
self.publish_object_as_marker(m)
except:
pass
def update(self):
try:
while not self.lock.empty():
updates = self.lock.get()
for object_state in updates.object_states: # type: ObjectState
object_name = object_state.object_id
if not self.get_world().has_object(object_name):
try:
world_object = WorldObject.from_object_state(
object_state)
except CorruptShapeException:
# skip objects without visual
continue
try:
self.get_world().add_object(world_object)
except pybullet.error as e:
logging.logwarn(
u'mesh \'{}\' does not exist'.format(
object_state.mesh_path))
continue
pose_in_map = transform_pose(MAP, object_state.pose).pose
self.get_world().set_object_pose(object_name, pose_in_map)
except Empty:
pass
return Status.RUNNING
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 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 cart_controller_to_goal(self, controller):
"""
:type controller: Controller
:return: (root_link, tip_link) -> {controller parameter -> value}
:rtype: dict
"""
goals = {}
root = controller.root_link
tip = controller.tip_link
controller.goal_pose = transform_pose(root, controller.goal_pose)
goals[root, tip] = controller
return goals
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 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 detach_object(self,
name,
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))
assert self.get_world().has_object(name)
compare_poses(self.get_world().get_object(name).base_pose,
p.pose,
decimal=2)
def move_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':
float(
axis_angle_from_quaternion(goal_pose.pose.orientation.x,
goal_pose.pose.orientation.y,
goal_pose.pose.orientation.z,
goal_pose.pose.orientation.w)[1])
}
self.send_and_check_joint_goal(js)
def move_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]
}
self.send_and_check_joint_goal(js)
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 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)
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 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 add_object(self, req):
"""
:type req: UpdateWorldRequest
"""
world_body = req.body
global_pose = from_pose_msg(
transform_pose(self.global_reference_frame_name, req.pose).pose)
if world_body.type is WorldBody.URDF_BODY:
#TODO test me
self.world.spawn_object_from_urdf_str(world_body.name,
world_body.urdf, global_pose)
else:
self.world.spawn_urdf_object(world_body_to_urdf_object(world_body),
global_pose)
# SUB-CASE: If it is an articulated object, open up a joint state subscriber
if world_body.joint_state_topic:
callback = (lambda msg: self.object_js_cb(world_body.name, msg))
self.object_js_subs[world_body.name] = \
rospy.Subscriber(world_body.joint_state_topic, JointState, callback, queue_size=1)
def call_back(pose_stamped):
"""
:type pose_stamped: PoseStamped
"""
rospy.loginfo(u'received simple move base goal')
goal = transform_pose(root, pose_stamped)
js = {
x_joint:
goal.pose.position.x,
y_joint:
goal.pose.position.y,
z_joint:
rotation_from_matrix(
quaternion_matrix([
goal.pose.orientation.x, goal.pose.orientation.y,
goal.pose.orientation.z, goal.pose.orientation.w
]))[0]
}
giskard.set_joint_goal(js)
giskard.plan_and_execute(wait=False)
def add_object(self, req):
"""
:type req: UpdateWorldRequest
"""
world_body = req.body
global_pose = transform_pose(self.map_frame, req.pose).pose
world_object = WorldObject.from_world_body(world_body)
self.get_world().add_object(world_object)
self.get_world().set_object_pose(world_body.name, global_pose)
try:
m = self.get_world().get_object(world_body.name).as_marker_msg()
m.header.frame_id = self.map_frame
self.publish_object_as_marker(m)
except:
pass
# SUB-CASE: If it is an articulated object, open up a joint state subscriber
# FIXME also keep track of base pose
if world_body.joint_state_topic:
callback = (lambda msg: self.object_js_cb(world_body.name, msg))
self.object_js_subs[world_body.name] = \
rospy.Subscriber(world_body.joint_state_topic, JointState, callback, queue_size=1)
def __init__(self, god_map, root_link, tip_link, goal, weight=HIGH_WEIGHT, gain=3, max_speed=0.1):
super(BasicCartesianConstraint, self).__init__(god_map)
self.root = root_link
self.tip = tip_link
goal = convert_dictionary_to_ros_message(u'geometry_msgs/PoseStamped', goal)
goal = transform_pose(self.root, goal)
# make sure rotation is normalized quaternion
# TODO make a function out of this
rotation = np.array([goal.pose.orientation.x,
goal.pose.orientation.y,
goal.pose.orientation.z,
goal.pose.orientation.w])
normalized_rotation = rotation / np.linalg.norm(rotation)
goal.pose.orientation.x = normalized_rotation[0]
goal.pose.orientation.y = normalized_rotation[1]
goal.pose.orientation.z = normalized_rotation[2]
goal.pose.orientation.w = normalized_rotation[3]
params = {self.goal: goal,
self.weight: weight,
self.gain: gain,
self.max_speed: max_speed}
self.save_params_on_god_map(params)
def test_pointing(self, better_pose):
tip = u'head_mount_kinect2_rgb_optical_frame'
goal_point = lookup_point(u'map', better_pose.r_tip)
better_pose.pointing(tip, goal_point)
better_pose.send_and_check_goal()
current_x = Vector3Stamped()
current_x.header.frame_id = tip
current_x.vector.z = 1
expected_x = transform_point(tip, goal_point)
np.testing.assert_almost_equal(expected_x.point.y, 0, 2)
np.testing.assert_almost_equal(expected_x.point.x, 0, 2)
goal_point = lookup_point(u'map', better_pose.r_tip)
better_pose.pointing(tip, goal_point, root=better_pose.r_tip)
r_goal = PoseStamped()
r_goal.header.frame_id = better_pose.r_tip
r_goal.pose.position.x -= 0.2
r_goal.pose.position.z -= 0.5
r_goal.pose.orientation.w = 1
r_goal = transform_pose(better_pose.default_root, r_goal)
r_goal.pose.orientation = Quaternion(*quaternion_from_matrix(
[[0, 0, 1, 0], [0, -1, 0, 0], [1, 0, 0, 0], [0, 0, 0, 1]]))
better_pose.set_and_check_cart_goal(r_goal, better_pose.r_tip,
u'base_footprint')
current_x = Vector3Stamped()
current_x.header.frame_id = tip
current_x.vector.z = 1
expected_x = lookup_point(tip, better_pose.r_tip)
np.testing.assert_almost_equal(expected_x.point.y, 0, 2)
np.testing.assert_almost_equal(expected_x.point.x, 0, 2)
