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, 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 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):
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, 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 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, 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, 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, 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):
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 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, 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))
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 __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 __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']
#!/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))
giskard_wrapper.attach_object(test_object_name, "hand_palm_link")
def detach_test_object():
if test_object_name in giskard_wrapper.get_attached_objects().object_names:
giskard_wrapper.detach_object(test_object_name)
def clear_world():
result = giskard_wrapper.clear_world()
rospy.loginfo("Clear world: {}".format(result.error_codes))
if __name__ == '__main__':
rospy.init_node("manipulation_test_marker")
giskard_wrapper = GiskardWrapper()
gripper = Gripper(
apply_force_action_server=u'/hsrb/gripper_controller/apply_force',
follow_joint_trajectory_server=
u'/hsrb/gripper_controller/follow_joint_trajectory')
# connect to servers
take_pose_client = actionlib.SimpleActionClient('take_pose_server',
TakePoseAction)
grasp_client = actionlib.SimpleActionClient('grasp_server', GraspAction)
place_client = actionlib.SimpleActionClient('place_server', PlaceAction)
open_client = actionlib.SimpleActionClient('open_server', OpenAction)
take_pose_client.wait_for_server()
grasp_client.wait_for_server()
place_client.wait_for_server()
open_client.wait_for_server()