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)
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 __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 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 kitchen_setup(zero_pose):
"""
:type resetted_giskard: GiskardTestWrapper
:return:
"""
object_name = u'kitchen'
zero_pose.add_urdf(object_name, rospy.get_param(u'kitchen_description'),
tf.lookup_pose(u'map', u'iai_kitchen/world'))
return zero_pose
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 set_cart_goal_for_basis(x, y, z):
giskard = GiskardWrapper()
# take base_footprint
poseStamp = lookup_pose("odom_combined", "base_footprint")
# set goal pose
poseStamp.pose.position.x = x
poseStamp.pose.position.y = y
poseStamp.pose.position.z = z
giskard.set_cart_goal("odom_combined", "base_footprint", poseStamp)
giskard.avoid_all_collisions()
giskard.plan_and_execute()
def kitchen_setup(resetted_giskard):
"""
:type resetted_giskard: GiskardTestWrapper
:return:
"""
resetted_giskard.allow_all_collisions()
resetted_giskard.send_and_check_joint_goal(gaya_pose)
object_name = u'kitchen'
resetted_giskard.add_urdf(object_name, rospy.get_param(u'kitchen_description'),
lookup_pose(u'map', u'iai_kitchen/world'), u'/kitchen/joint_states')
js = {k: 0.0 for k in resetted_giskard.get_world().get_object(object_name).get_movable_joints()}
resetted_giskard.set_kitchen_js(js)
return resetted_giskard
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 execute_cb(self, goal):
# uncomment to disable collision avoidance
# self._manipulator.set_collision(None)
rospy.loginfo("Grasping: {}".format(goal))
# Set initial result value.
success = True
self._result.error_code = self._result.FAILED
collision_whitelist = []
if goal.object_frame_id in self._giskard_wrapper.get_object_names(
).object_names:
collision_whitelist.append(goal.object_frame_id)
else:
rospy.logwarn("unknown object: {}".format(goal.object_frame_id))
# get current robot_pose
robot_pose = tfwrapper.lookup_pose('map', 'base_footprint')
# open gripper
self._gripper.set_gripper_joint_position(1.2)
success &= self._manipulator.move_to_goal(
root_link=self._root,
tip_link=u'hand_gripper_tool_frame',
goal_pose=goal.goal_pose,
robot_pose=robot_pose,
mode=goal.grasp_mode,
step=0.1,
collision_whitelist=collision_whitelist)
if success:
self._gripper.close_gripper_force(0.8)
success &= self._gripper.object_in_gripper()
if success:
# Attach object
if goal.object_frame_id in self._giskard_wrapper.get_object_names(
).object_names:
self._giskard_wrapper.attach_object(
goal.object_frame_id, u'hand_palm_link')
self._gripper.publish_object_in_gripper(
goal.object_frame_id, goal.goal_pose,
ObjectInGripper.GRASPED)
robot_pose.header.stamp = rospy.Time.now(
) # Might not be needed but is cleaner this way
success &= self._manipulator.move_to_goal(root_link=self._root,
tip_link=u'base_footprint',
goal_pose=robot_pose)
success &= self._manipulator.take_robot_pose(
rospy.get_param(u'/manipulation/robot_poses/transport'))
if success:
self._result.error_code = self._result.SUCCESS
self._as.set_succeeded(self._result)
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 kitchen_setup(better_pose):
# better_pose.allow_all_collisions()
# better_pose.send_and_check_joint_goal(gaya_pose)
object_name = u'kitchen'
better_pose.add_urdf(object_name,
rospy.get_param(u'kitchen_description'),
tf.lookup_pose(u'map', u'iai_kitchen/world'),
u'/kitchen/joint_states',
set_js_topic=u'/kitchen/cram_joint_states')
js = {
k: 0.0
for k in better_pose.get_world().get_object(
object_name).get_movable_joints()
}
better_pose.set_kitchen_js(js)
return better_pose
def update(self):
try:
if self.mjs is None:
js = self.lock.get()
else:
js = self.lock.get_nowait()
self.mjs = to_joint_state_dict(js)
except Empty:
pass
robot_frame = self.get_robot().get_root()
base_pose = lookup_pose(self.map_frame, robot_frame)
self.get_robot().base_pose = base_pose.pose
self.god_map.safe_set_data(identifier.joint_states, self.mjs)
return Status.SUCCESS
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 cb(dt):
pose = lookup_pose(odom, base_footprint)
js = JointState()
js.header.stamp = pose.header.stamp
js.name = [x, y, z]
js.position = [
pose.pose.position.x, pose.pose.position.y,
rotation_from_matrix(
quaternion_matrix([
pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w
]))[0]
]
js.velocity = [0, 0, 0]
js.effort = [0, 0, 0]
pose_pub.publish(js)
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 execute_cb(self, goal):
rospy.loginfo("Making plan: {}".format(goal))
self._result = MakePlanResult()
self._result.error_code = self._result.FAILED
# TODO move robot in start pose currently replaced with actual robot pose
robot_pose = tfwrapper.lookup_pose('map', 'base_footprint')
# Prepare object
if goal.action_mode == goal.PLACE:
if goal.object_frame_id not in self._giskard_wrapper.get_attached_objects().object_names:
rospy.loginfo("object not attached to gripper: {}. Creating dummy object".format(goal.object_frame_id))
if self.dummy_object_ in self._giskard_wrapper.get_object_names().object_names:
self._giskard_wrapper.remove_object(self.dummy_object_)
self._giskard_wrapper.add_box(self.dummy_object_,
[goal.object_size.x, goal.object_size.y, goal.object_size.z],
self.gripper_frame_)
self._giskard_wrapper.attach_object(self.dummy_object_, self.gripper_frame_)
elif goal.action_mode == goal.GRASP:
if goal.object_frame_id in self._giskard_wrapper.get_object_names().object_names:
self._giskard_wrapper.allow_collision(body_b=goal.object_frame_id)
else:
rospy.logerr("Unknown action_mode: {}".format(goal.action_mode))
self._as.set_succeeded(self._result)
return
if goal.gripper_mode in [goal.FRONT, goal.TOP]:
goal.goal_pose.pose.orientation = Quaternion(
*quaternion_multiply(to_tf_quaternion(robot_pose.pose.orientation),
self.mode_rotation_[goal.gripper_mode]))
self._giskard_wrapper.set_cart_goal(self.root_frame_, self.gripper_frame_, goal.goal_pose)
self._giskard_wrapper.plan()
result = self._giskard_wrapper.get_result()
# Clean up dummy object
if self.dummy_object_ in self._giskard_wrapper.get_attached_objects().object_names:
self._giskard_wrapper.detach_object(self.dummy_object_)
self._giskard_wrapper.remove_object(self.dummy_object_)
# Clean up collission excemption for goal_object
self._giskard_wrapper.avoid_all_collisions()
if result.SUCCESS in result.error_codes:
self._result.error_code = self._result.SUCCESS
self._as.set_succeeded(self._result)
def execute_cb(self, goal):
# uncomment to disable collision avoidance
# self._manipulator.set_collision(None)
rospy.loginfo("Placing: {}".format(goal))
# Set initial result value.
success = True
self._result.error_code = self._result.FAILED
if goal.object_frame_id not in self._giskard_wrapper.get_attached_objects().object_names:
rospy.logwarn("object not attached to gripper: {}".format(goal.object_frame_id))
# get current robot_pose
robot_pose = tfwrapper.lookup_pose('map', 'base_footprint')
success &= self._manipulator.move_to_goal(root_link=self._root,
tip_link=u'hand_gripper_tool_frame',
goal_pose=goal.goal_pose,
robot_pose=robot_pose,
mode=goal.place_mode,
step=0.1)
if success:
self._gripper.set_gripper_joint_position(1.2)
if goal.object_frame_id in self._giskard_wrapper.get_attached_objects().object_names:
self._giskard_wrapper.detach_object(goal.object_frame_id)
self._gripper.publish_object_in_gripper(goal.object_frame_id, goal.goal_pose, ObjectInGripper.PLACED)
collision_whitelist = []
if goal.object_frame_id in self._giskard_wrapper.get_object_names().object_names:
collision_whitelist.append(goal.object_frame_id)
else:
rospy.logwarn("unknown object: {}".format(goal.object_frame_id))
robot_pose.header.stamp = rospy.Time.now() # Might not be needed but is cleaner this way
success &= self._manipulator.move_to_goal(root_link=self._root,
tip_link=u'base_footprint',
goal_pose=robot_pose,
collision_whitelist=collision_whitelist)
success &= self._manipulator.take_robot_pose(rospy.get_param(u'/manipulation/robot_poses/neutral'))
if success:
self._result.error_code = self._result.SUCCESS
self._as.set_succeeded(self._result)
#! /usr/bin/env python
import rospy
from giskardpy.python_interface import GiskardWrapper
from giskardpy import tfwrapper
rospy.init_node('add_static_giskard')
giskard_wrapper = GiskardWrapper()
_urdf = rospy.get_param('hsrb_lab', False)
kitchen_frame = rospy.get_param('~environment_frame', 'iai_kitchen/world')
while not _urdf:
rospy.sleep(5)
_urdf = rospy.get_param('hsrb_lab', False)
giskard_wrapper.detach_object('gripper_dummy')
giskard_wrapper.remove_object('gripper_dummy')
giskard_wrapper.remove_object('lab')
p = tfwrapper.lookup_pose('map', kitchen_frame)
#giskard_wrapper.attach_box(name='gripper_dummy', size=[0.05, 0.1, 0.1], frame_id="hand_palm_link",
# position=[0.05, 0, 0.05], orientation=[0, 0, 0, 1])
giskard_wrapper.add_urdf(name='lab', urdf=_urdf, js_topic='/kitchen/joint_states', pose=p)
def test_avoid_self_collision2(self, self_collision_pose):
self_collision_pose.send_and_check_goal()
map_T_root = lookup_pose(u'map', u'base_footprint')
expected_pose = Pose()
expected_pose.orientation.w = 1
compare_poses(map_T_root.pose, expected_pose)
from giskardpy.tfwrapper import lookup_transform, lookup_pose
from giskardpy import logging
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_pose(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:
logging.logwarn('neither _param nor _path specified')
sys.exit()
else:
