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 __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)
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
MoveGripperAction,
execute_cb=self.execute_cb,
auto_start=False)
self._giskard_wrapper = GiskardWrapper()
self._as.start()
def joint_space_client():
angle = 0.0
giskard = GiskardWrapper()
rospy.sleep(0.5)
# Creates the SimpleActionClient, passing the type of the action
# (MoveAction) to the constructor.
# Creates a goal to send to the action server.
# Waits until the action server has started up and started
# listening for goals.
angle += 0.8
rospy.loginfo("%f", angle)
gaya_pose = {'r_shoulder_pan_joint': -1.7125,
'r_shoulder_lift_joint': -0.25672,
'r_upper_arm_roll_joint': -1.46335,
'r_elbow_flex_joint': -2.12216,
'r_forearm_roll_joint': 1.76632,
'r_wrist_flex_joint': -0.10001,
'r_wrist_roll_joint': 0.05106,
'l_shoulder_pan_joint': 1.9652,
'l_shoulder_lift_joint': - 0.26499,
'l_upper_arm_roll_joint': 1.3837,
'l_elbow_flex_joint': - 2.1224,
'l_forearm_roll_joint': 16.99,
'l_wrist_flex_joint': - 0.10001,
'l_wrist_roll_joint': 0}
giskard.allow_all_collisions()
giskard.set_joint_goal(gaya_pose)
giskard.plan_and_execute()
return giskard.get_result()
def __init__(self, collision_distance=0.05, mode_rotation=None):
"""
Proxy class working with giskard for manipulating object with a gripper
:param collision_distance: distance to keep from objects None or < 0 allow all collisions
:type collision_distance:float
:param mode_rotation: Rotation of tip depending on robot_pose
:type mode_rotation: dict(mode: [x, y, z, w])
"""
self.mode_rotation_ = mode_rotation
self.collision_distance_ = collision_distance
self.giskard_wrapper_ = GiskardWrapper()
def __init__(self, name):
self._action_name = name
self.dummy_object_ = u'dummy_plan_object'
self.gripper_frame_ = u'hand_palm_link'
self.root_frame_ = u'odom'
self._as = actionlib.SimpleActionServer(self._action_name, MakePlanAction, execute_cb=self.execute_cb,
auto_start=False)
self._giskard_wrapper = GiskardWrapper()
self.mode_rotation_ = self.get_mode_rotation()
self._as.start()
rospy.loginfo("{} is ready and waiting for orders.".format(self._action_name))
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name, PlaceAction, execute_cb=self.execute_cb,
auto_start=False)
self._giskard_wrapper = GiskardWrapper()
self._gripper = Gripper(apply_force_action_server=u'/hsrb/gripper_controller/apply_force',
follow_joint_trajectory_server=u'/hsrb/gripper_controller/follow_joint_trajectory')
self._manipulator = Manipulator(mode_rotation=self.get_mode_rotation())
self._as.start()
if tfwrapper.tfBuffer is None:
tfwrapper.init()
rospy.loginfo("{} is ready and waiting for orders.".format(self._action_name))
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
TakePoseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
self._giskard_wrapper = GiskardWrapper()
self._robot = hsrb_interface.Robot()
self._whole_body = self._robot.get('whole_body')
rospy.loginfo("{} is ready and waiting for orders.".format(
self._action_name))
def __init__(self, apply_force_action_server,
follow_joint_trajectory_server):
self._gripper_apply_force_client = actionlib.SimpleActionClient(
apply_force_action_server, GripperApplyEffortAction)
self._gripper_apply_force_client.wait_for_server()
self._gripper_controller = actionlib.SimpleActionClient(
follow_joint_trajectory_server, FollowJointTrajectoryAction)
self._gripper_controller.wait_for_server()
self._obj_in_gripper_pub = rospy.Publisher("object_in_gripper",
ObjectInGripper,
queue_size=10)
self._giskard_wrapper = GiskardWrapper()
def __init__(self):
self.stage = 0
self.giskard = GiskardWrapper()
self.giskard.allow_all_collisions()
self.pose0 = {'torso_lift_joint': 0.20}
self.pose3 = {
'r_shoulder_pan_joint': 0.30,
'r_shoulder_lift_joint': 0.0,
'r_upper_arm_roll_joint': -1.95,
'r_elbow_flex_joint': -0.55,
# 'r_forearm_roll_joint': 1.76632,
# 'r_wrist_flex_joint': -0.10001,
'r_wrist_roll_joint': -1.55,
'l_shoulder_pan_joint': 0.25,
'l_shoulder_lift_joint': 1.05,
'l_upper_arm_roll_joint': 1.75,
'l_elbow_flex_joint': -1.75,
# 'l_forearm_roll_joint': 16.99,
# 'l_wrist_flex_joint': - 0.10001,
'l_wrist_roll_joint': 0.
}
# self.default_pose = {'r_shoulder_pan_joint': -1.7125,
# 'r_shoulder_lift_joint': -0.25672,
# 'r_upper_arm_roll_joint': -1.46335,
# 'r_elbow_flex_joint': -2.12216,
# 'r_forearm_roll_joint': 1.76632,
# 'r_wrist_flex_joint': -0.10001,
# 'r_wrist_roll_joint': 0.05106,
#
# 'l_shoulder_pan_joint': 1.9652,
# 'l_shoulder_lift_joint': - 0.26499,
# 'l_upper_arm_roll_joint': 1.3837,
# 'l_elbow_flex_joint': - 2.1224,
# 'l_forearm_roll_joint': 16.99,
# 'l_wrist_flex_joint': - 0.10001,
# 'l_wrist_roll_joint': 0}
self.vel_pub = rospy.Publisher('/base_controller/command',
Twist,
queue_size=1)
# self.DemoCommandsSub = rospy.Subscriber("/DemoCommands",
# String,
# self.DemoCommandsCallback,
# queue_size = 1)
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
# print "start waiting"
# rospy.wait_for_service('SimulationCommands')
# print "ready"
self.run_demo()
def __init__(self):
description = get_param('robot_description')
self.giskard_wrapper = GiskardWrapper()
self.free_joints = {}
self.joint_list = [
] # for maintaining the original order of the joints
self.dependent_joints = get_param("dependent_joints", {})
self.use_mimic = get_param('~use_mimic_tags', True)
self.use_small = get_param('~use_smallest_joint_limits', True)
self.zeros = get_param("zeros")
#self.pub_def_positions = get_param("publish_default_positions", True)
#self.pub_def_vels = get_param("publish_default_velocities", False)
#self.pub_def_efforts = get_param("publish_default_efforts", False)
msg = rospy.wait_for_message(u'/whole_body_controller/state',
JointTrajectoryControllerState)
self.giskard_joints = msg.joint_names
robot = minidom.parseString(description)
if robot.getElementsByTagName('COLLADA'):
self.init_collada(robot)
else:
self.init_urdf(robot)
def __init__(self, root_tips, suffix=u''):
"""
:param root_tips: list containing root->tip tuple for each interactive marker.
:type root_tips: list
:param suffix: the marker will be called 'eef_control{}'.format(suffix)
:type suffix: str
"""
self.enable_self_collision = rospy.get_param(u'~enable_self_collision',
True)
self.giskard = GiskardWrapper()
if len(root_tips) > 0:
self.roots, self.tips = zip(*root_tips)
else:
self.roots = []
self.tips = []
self.suffix = suffix
self.markers = {}
# marker server
self.server = InteractiveMarkerServer(u'eef_control{}'.format(
self.suffix))
self.menu_handler = MenuHandler()
for root, tip in zip(self.roots, self.tips):
int_marker = self.make_6dof_marker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip)
self.server.insert(
int_marker,
self.process_feedback(self.server, int_marker.name, root, tip,
self.giskard,
self.enable_self_collision))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
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")
class MoveGripperServer:
_feedback = MoveGripperFeedback()
_result = MoveGripperResult()
_root = u'odom'
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
MoveGripperAction,
execute_cb=self.execute_cb,
auto_start=False)
self._giskard_wrapper = GiskardWrapper()
self._as.start()
def execute_cb(self, goal):
rospy.loginfo("Move gripper: {}".format(goal))
self._result.error_code = self._result.FAILED
self._giskard_wrapper.interrupt()
self._giskard_wrapper.set_cart_goal(self._root, u'hand_palm_link',
goal.goal_pose)
self._giskard_wrapper.plan_and_execute(wait=False)
result = self._giskard_wrapper.get_result(rospy.Duration(60))
if result and result.SUCCESS in result.error_code:
self._result.error_code = self._result.SUCCESS
self._as.set_succeeded(self._result)
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 __init__(self):
rospy.set_param(u'~enable_gui', False)
rospy.set_param(u'~interactive_marker_chains', [])
rospy.set_param(u'~map_frame', u'map')
rospy.set_param(u'~joint_convergence_threshold', 0.002)
rospy.set_param(u'~wiggle_precision_threshold', 7)
rospy.set_param(u'~sample_period', 0.1)
rospy.set_param(u'~default_joint_vel_limit', 10)
rospy.set_param(u'~default_collision_avoidance_distance', 0.05)
rospy.set_param(u'~fill_velocity_values', False)
rospy.set_param(u'~nWSR', u'None')
rospy.set_param(u'~root_link', u'base_footprint')
rospy.set_param(u'~enable_collision_marker', True)
rospy.set_param(u'~enable_self_collision', False)
rospy.set_param(u'~path_to_data_folder', u'../data/pr2/')
rospy.set_param(u'~collision_time_threshold', 15)
rospy.set_param(u'~max_traj_length', 30)
self.sub_result = rospy.Subscriber(u'/qp_controller/command/result',
MoveActionResult,
self.cb,
queue_size=100)
self.pm = giskard_pm()
self.pm.start_plugins()
self.wrapper = GiskardWrapper(ns=u'tests')
self.results = Queue(100)
self.robot = self.pm._plugins[u'fk'].get_robot()
self.controlled_joints = self.pm._plugins[
u'controlled joints'].controlled_joints
self.joint_limits = {
joint_name: self.robot.get_joint_lower_upper_limit(joint_name)
for joint_name in self.controlled_joints
if self.robot.is_joint_controllable(joint_name)
}
self.world = self.pm._plugins[u'bullet'].world # type: PyBulletWorld
self.default_root = u'base_link'
self.r_tip = u'r_gripper_tool_frame'
self.l_tip = u'l_gripper_tool_frame'
self.map = u'map'
self.simple_base_pose_pub = rospy.Publisher(u'/move_base_simple/goal',
PoseStamped,
queue_size=10)
rospy.sleep(1)
def __init__(self):
self.stage = 0
self.giskard = GiskardWrapper()
self.giskard.allow_all_collisions()
self.pose0 = {'torso_lift_joint': 0.15}
self.vel_pub = rospy.Publisher('/base_controller/command',
Twist,
queue_size=1)
# self.DemoCommandsSub = rospy.Subscriber("/DemoCommands",
# String,
# self.DemoCommandsCallback,
# queue_size = 1)
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
# print "start waiting"
# rospy.wait_for_service('SimulationCommands')
# print "ready"
self.run_demo()
def __init__(self):
# initialize object
print("HSR_MOVE is actived")
# Set Mobility checker
self.check_mobility = Mobility()
self.interface = GiskardWrapper()
self.pose_stamped = PoseStamped()
self.states_pre_grasp = {
"wrist_roll_joint": 0.0,
"wrist_flex_joint": 0.0,
"arm_roll_joint": 0.0,
"arm_flex_joint": 0.0,
"arm_lift_joint": 0.0
}
self.place_states = {
"wrist_roll_joint": 0.0,
"wrist_flex_joint": -1.57,
"arm_roll_joint": 0.0,
"arm_flex_joint": 0.0,
"arm_lift_joint": 0.0
}
self.primary_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": -0.026,
"arm_roll_joint": -1.57,
"wrist_flex_joint": -1.57,
"wrist_roll_joint": 0.0
}
self.end_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": 0.0,
"wrist_flex_joint": -1.0,
"arm_roll_joint": 1.57
}
self.perceive_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": 0.0,
"wrist_flex_joint": -1.70,
"arm_roll_joint": 1.57
}
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 __init__(self, enabled=True, knowrob=None, avoid_self_collisinon=True, tip='camera_link',
root='ur5_shoulder_link'):
self.move_time_limit = 25
self.enabled = enabled
self.knowrob = knowrob
self.giskard = GiskardWrapper()
self.tip = tip
self.root = root
self.trans_p_gain = 0.5
self.rot_p_gain = 0.5
self.trans_max_speed = 0.1
self.rot_max_speed = 0.3
self.self_collision_min_dist = 0.03
self.avoid_self_collision = avoid_self_collisinon
# TODO get this from param server of topic
self.joint_names = ['ur5_shoulder_pan_joint',
'ur5_shoulder_lift_joint',
'ur5_elbow_joint',
'ur5_wrist_1_joint',
'ur5_wrist_2_joint',
'ur5_wrist_3_joint', ]
def __init__(self, enabled=True, knowrob=None, avoid_self_collisinon=True, tip='camera_link',
root='iiwa_link_0'):
self.move_time_limit = 25
self.enabled = enabled
self.knowrob = knowrob
self.giskard = GiskardWrapper()
self.tip = tip
self.root = root
self.trans_p_gain = 0.5
self.rot_p_gain = 0.5
self.trans_max_speed = 0.1
self.rot_max_speed = 0.3
self.self_collision_min_dist = 0.03
self.avoid_self_collision = avoid_self_collisinon
# TODO get this from param server of topic
self.joint_names = ['iiwa_joint_1',
'iiwa_joint_2',
'iiwa_joint_3',
'iiwa_joint_4',
'iiwa_joint_5',
'iiwa_joint_6',
'iiwa_joint_7']
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
OpenAction,
execute_cb=self.execute_cb,
auto_start=False)
self._gripper = Gripper(
apply_force_action_server=u'/hsrb/gripper_controller/apply_force',
follow_joint_trajectory_server=
u'/hsrb/gripper_controller/follow_joint_trajectory')
self._manipulator = Manipulator(collision_distance=0.01)
self._giskard_wrapper = GiskardWrapper()
self._as.start()
rospy.loginfo("{} is ready and waiting for orders.".format(
self._action_name))
class Demo:
def __init__(self):
self.stage = 0
self.giskard = GiskardWrapper()
self.giskard.allow_all_collisions()
self.pose0 = {'torso_lift_joint': 0.15}
self.vel_pub = rospy.Publisher('/base_controller/command',
Twist,
queue_size=1)
# self.DemoCommandsSub = rospy.Subscriber("/DemoCommands",
# String,
# self.DemoCommandsCallback,
# queue_size = 1)
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
# print "start waiting"
# rospy.wait_for_service('SimulationCommands')
# print "ready"
self.run_demo()
def run_demo(self):
try:
print "stage 0"
self.giskard.set_joint_goal(self.pose0)
# self.giskard.add_cmd()
l_tip = u'r_gripper_tool_frame'
self.giskard.plan_and_execute()
print "stage 1"
rospy.sleep(0.5)
self.stage = -1
except KeyboardInterrupt:
print('interrupted!')
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)
#!/usr/bin/env python
import rospy
from giskardpy.python_interface import GiskardWrapper
from giskardpy import logging
if __name__ == '__main__':
rospy.init_node('clear_world')
giskard = GiskardWrapper()
result = giskard.clear_world()
if result.error_codes == result.SUCCESS:
logging.loginfo('clear world')
else:
logging.logwarn('failed to clear world {}'.format(result))
#!/usr/bin/env python
import rospy
import sys
from giskardpy.python_interface import GiskardWrapper
if __name__ == '__main__':
rospy.init_node('add_cylinder')
giskard = GiskardWrapper()
try:
name = rospy.get_param('~name')
result = giskard.add_cylinder(name=name,
size=rospy.get_param('~size', (1, 1)),
frame_id=rospy.get_param('~frame', 'map'),
position=rospy.get_param('~position', (0, 0, 0)),
orientation=rospy.get_param('~orientation', (0, 0, 0, 1)))
if result.error_codes == result.SUCCESS:
rospy.loginfo('cylinder \'{}\' added'.format(name))
else:
rospy.logwarn('failed to add cylinder \'{}\''.format(name))
except KeyError:
rospy.loginfo(
'Example call: rosrun giskardpy add_cylinder.py _name:=cylinder _size:=[1,1] _frame_id:=map _position:=[0,0,0] _orientation:=[0,0,0,1]')
#!/usr/bin/env python
import rospy
import sys
from giskardpy.python_interface import GiskardWrapper
if __name__ == '__main__':
rospy.init_node('remove_object')
giskard = GiskardWrapper([])
try:
name = rospy.get_param('~name')
except KeyError:
try:
name = sys.argv[1]
except IndexError:
rospy.loginfo(
'Example call: rosrun giskardpy remove_object.py _name:=muh')
sys.exit()
result = giskard.remove_object(name)
if result.error_codes == result.SUCCESS:
rospy.loginfo('{} removed'.format(name))
else:
rospy.logwarn('failed to remove {} {}'.format(name, result))
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)
if __name__ == '__main__':
try:
rospy.init_node(u'move_base_simple_goal')
init()
x_joint = u'odom_x_joint'
y_joint = u'odom_y_joint'
z_joint = u'odom_z_joint'
root = u'odom_combined'
giskard = GiskardWrapper()
sub = rospy.Subscriber(u'/move_base_simple/goal',
PoseStamped,
call_back,
queue_size=10)
rospy.spin()
except rospy.ROSInterruptException:
pass
class MoveArm(object):
def __init__(self, enabled=True, knowrob=None, avoid_self_collisinon=True, tip='camera_link',
root='ur5_shoulder_link'):
self.move_time_limit = 25
self.enabled = enabled
self.knowrob = knowrob
self.giskard = GiskardWrapper()
self.tip = tip
self.root = root
self.trans_p_gain = 0.5
self.rot_p_gain = 0.5
self.trans_max_speed = 0.1
self.rot_max_speed = 0.3
self.self_collision_min_dist = 0.03
self.avoid_self_collision = avoid_self_collisinon
# TODO get this from param server of topic
self.joint_names = ['ur5_shoulder_pan_joint',
'ur5_shoulder_lift_joint',
'ur5_elbow_joint',
'ur5_wrist_1_joint',
'ur5_wrist_2_joint',
'ur5_wrist_3_joint', ]
def set_translation_goal(self, translation):
goal_pose = PoseStamped()
if isinstance(translation, PointStamped):
goal_pose.header = translation.header
goal_pose.pose.position = translation.point
else:
goal_pose = translation
self.giskard.set_translation_goal(self.root, self.tip, goal_pose, self.trans_p_gain, self.trans_max_speed)
def set_orientation_goal(self, orientation):
goal_pose = PoseStamped()
if isinstance(orientation, QuaternionStamped):
goal_pose.header = orientation.header
goal_pose.pose.orientation = orientation.quaternion
else:
goal_pose = orientation
self.giskard.set_rotation_goal(self.root, self.tip, goal_pose, self.rot_p_gain, self.rot_max_speed)
def set_default_self_collision_avoidance(self):
if not self.avoid_self_collision:
self.giskard.allow_self_collision()
else:
self.giskard.set_self_collision_distance(self.self_collision_min_dist)
self.giskard.allow_collision(['ur5_wrist_3_link'], self.giskard.get_robot_name(), ['ur5_forearm_link'])
def set_and_send_cartesian_goal(self, goal_pose):
self.set_translation_goal(goal_pose)
self.set_orientation_goal(goal_pose)
self.set_default_self_collision_avoidance()
return self.giskard.plan_and_execute().error_code == MoveResult.SUCCESS
def send_cartesian_goal(self):
if self.enabled:
self.set_default_self_collision_avoidance()
return self.giskard.plan_and_execute().error_code == MoveResult.SUCCESS
def set_and_send_joint_goal(self, joint_state):
if self.enabled:
self.giskard.set_joint_goal(joint_state)
self.set_default_self_collision_avoidance()
return self.giskard.plan_and_execute().error_code == MoveResult.SUCCESS
def move_absolute(self, target_pose, retry=True):
if self.enabled:
self.giskard.set_cart_goal('odom', 'base_footprint', target_pose)
return self.giskard.plan_and_execute()
# self.goal_pub.publish(target_pose)
# goal = MoveBaseGoal()
# goal.target_pose = target_pose
# if self.knowrob is not None:
# self.knowrob.start_base_movement(self.knowrob.pose_to_prolog(target_pose))
# while True:
# self.client.send_goal(goal)
# wait_result = self.client.wait_for_result(rospy.Duration(self.timeout))
# result = self.client.get_result()
# state = self.client.get_state()
# if wait_result and state == GoalStatus.SUCCEEDED:
# break
# if retry:
# cmd = raw_input('base movement did not finish in time, retry? [y/n]')
# retry = cmd == 'y'
# if not retry:
# print('movement did not finish in time')
# if self.knowrob is not None:
# self.knowrob.finish_action()
# raise TimeoutError()
# if self.knowrob is not None:
# self.knowrob.finish_action()
# return result
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()
# self.goal_pub.publish(target_pose)
# goal = MoveBaseGoal()
# goal.target_pose = target_pose
# if self.knowrob is not None:
# self.knowrob.start_base_movement(self.knowrob.pose_to_prolog(target_pose))
# while True:
# self.client.send_goal(goal)
# wait_result = self.client.wait_for_result(rospy.Duration(self.timeout))
# result = self.client.get_result()
# state = self.client.get_state()
# if wait_result and state == GoalStatus.SUCCEEDED:
# break
# if retry:
# cmd = raw_input('base movement did not finish in time, retry? [y/n]')
# retry = cmd == 'y'
# if not retry:
# print('movement did not finish in time')
# if self.knowrob is not None:
# self.knowrob.finish_action()
# raise TimeoutError()
# if self.knowrob is not None:
# self.knowrob.finish_action()
# return result
def cam_pose_to_base_pose(self, pose, frame):
"""
:type pose: PoseStamped
:rtype: PoseStamped
"""
T_shelf___cam_joint_g = msg_to_kdl(pose)
T_map___bfg = T_shelf___cam_joint_g * self.get_frame_in_base_footprint_kdl(frame).Inverse()
base_pose = kdl_to_posestamped(T_map___bfg, pose.header.frame_id)
return base_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 place_pose_left(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
-1.57,
-1.39,
-2.4,
0.46,
1.57,
-1.57,
]
return self.set_and_send_joint_goal(joint_state)
def place_pose_right(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
-np.pi / 2,
-2.44177755311,
2.15026930371,
0.291547812391,
np.pi / 2,
np.pi / 2,
]
return self.set_and_send_joint_goal(joint_state)
def floor_detection_pose_right(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
np.pi,
-1.37,
0.51,
-0.72,
-0.22,
0,
]
return self.set_and_send_joint_goal(joint_state)
def floor_detection_pose_left(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
0.0,
-1.768,
-0.51,
-2.396,
0.243438,
-np.pi,
]
return self.set_and_send_joint_goal(joint_state)
def drive_pose(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
np.pi * 1.5,
-np.pi / 2,
-2.3,
-np.pi / 2,
0,
-np.pi / 2,
]
return self.set_and_send_joint_goal(joint_state)
def pre_baseboard_pose(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
0.0,
-1.6460,
-2.171,
-0.85549,
0.2181,
-3.19172,
]
return self.set_and_send_joint_goal(joint_state)
def see_pose(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
# -1.57114202181,
# -1.44927913347,
# -1.25000602404,
# -4.01274043718,
# -1.56251222292,
# 1.62433183193,
-np.pi/2,
-2.18,
1.47,
1.03,
np.pi/2,
np.pi/2,
]
return self.set_and_send_joint_goal(joint_state)
class PlaceServer:
_feedback = PlaceFeedback()
_result = PlaceResult()
_root = u'odom'
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name, PlaceAction, execute_cb=self.execute_cb,
auto_start=False)
self._giskard_wrapper = GiskardWrapper()
self._gripper = Gripper(apply_force_action_server=u'/hsrb/gripper_controller/apply_force',
follow_joint_trajectory_server=u'/hsrb/gripper_controller/follow_joint_trajectory')
self._manipulator = Manipulator(mode_rotation=self.get_mode_rotation())
self._as.start()
if tfwrapper.tfBuffer is None:
tfwrapper.init()
rospy.loginfo("{} is ready and waiting for orders.".format(self._action_name))
def get_mode_rotation(self):
mode_rotation = {}
front_rotation = rospy.get_param(u'/manipulation/base_gripper_rotation/front', default=None)
top_rotation = rospy.get_param(u'/manipulation/base_gripper_rotation/top', default=None)
if front_rotation:
mode_rotation[PlaceGoal.FRONT] = front_rotation
if top_rotation:
mode_rotation[PlaceGoal.TOP] = top_rotation
return mode_rotation
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)