def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
self.client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.client.moveToJointPosition(joints,
pose,
0.0,
max_velocity_scaling_factor=0.5)
if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
if move_thread:
move_thread.stop()
# On success quit
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("done")
sys.exit(0)
return
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def __init__(self, tower_array, disk_array, config, debug=0):
print "[INFO] Initialise World"
self.DEBUG = debug
# initialise arrays
self.tower_array = tower_array
self.disk_array = disk_array
# configs
self.max_gripper = config.get_max_gripper()
self.disk_height = config.disk_height
self.tower_positions = config.tower_positions
self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def clean_up(self, close=False):
if close:
# get the actual list after game
self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
for name in self.planning_scene_interface.getKnownCollisionObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing object %s" % name)
self.planning_scene_interface.removeCollisionObject(name, False)
for name in self.planning_scene_interface.getKnownAttachedObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing attached object %s" % name)
self.planning_scene_interface.removeAttachedObject(name, False)
if close:
self.planning_scene_interface.waitForSync(5.0)
pass
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
rospy.loginfo("...connected.")
self.scene = PlanningSceneInterface("base_link")
self.attached_object_publisher = rospy.Publisher(
"attached_collision_object", AttachedCollisionObject, queue_size=10
)
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
self.debug_index = -1
self.graspables = []
def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
self.client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.client.moveToJointPosition(
joints, pose, 0.0, max_velocity_scaling_factor=0.5)
if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
if move_thread:
move_thread.stop()
# On success quit
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("done")
sys.exit(0)
return
class GripperInterfaceClient():
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action",
GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def execute_trajectory(self, trajectory):
goal_position = trajectory.points[-1].positions
print '==========', goal_position
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position[0]+goal_position[1]
command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this
self.client.send_goal(command_goal)
self.client.wait_for_result()
def move_to(self, goal_position, max_effort):
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position
command_goal.command.max_effort = max_effort
self.client.send_goal(command_goal)
self.client.wait_for_result()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
# Try mesh representation
#idx = -1
#for obj in find_result.objects:
# idx += 1
# obj.object.name = "object%d"%idx
# self.scene.addMesh(obj.object.name,
# obj.object.mesh_poses[0],
# obj.object.meshes[0],
# wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0] + 0.1,
2.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
# if obj.object.primitives[0].dimensions[0] < 0.03 or \
# obj.object.primitives[0].dimensions[0] > 0.04 or \
# obj.object.primitives[0].dimensions[0] < 0.07 or \
# obj.object.primitives[0].dimensions[0] > 0.09 or \
# obj.object.primitives[2].dimensions[2] < 0.19 or \
# obj.object.primitives[2].dimensions[2] > 0.20:
# continue
# has to be on table
#if obj.object.primitive_poses[0].position.z < 0.5:
# continue
return obj.object, obj.grasps
# nothing detected
return None, None
def plan_pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
support_name=block.support_surface,
allow_gripper_support_collision=False,
planner_id = 'PRMkConfigDefault',
retries = 2,
scene=self.scene)
#self.pick_result = pick_result
if success:
return pick_result.trajectory_stages
else:
rospy.loginfo("Planning Failed.")
return None
# Note: fetch_moveit_config move_group.launch must be running
# Safety!: Do NOT run this script near people or objects.
# Safety!: There is NO perception.
# The ONLY objects the collision detection software is aware
# of are itself & the floor.
if __name__ == '__main__':
rospy.init_node("simple_disco")
# Create move group interface for a fetch robot
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TF joint names
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
class FetchRobotApi:
def __init__(self):
self.moving_mode = False
self.waldo_mode = False
self.prev_joy_buttons = 0
# See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
self.joint_names = [
#'torso_lift_joint',
#'bellows_joint',
#'head_pan_joint',
#'head_tilt_joint',
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
'l_gripper_finger_joint',
#'r_gripper_finger_joint',
]
self.joint_name_map = dict([
(name, index) for index, name in enumerate(self.joint_names)
])
self.cur_joint_pos = np.zeros([len(self.joint_names)])
self.cur_joint_vel = np.zeros([len(self.joint_names)])
self.cur_joint_effort = np.zeros([len(self.joint_names)])
self.cur_base_scan = np.zeros([662], dtype=np.float32)
self.cur_head_camera_depth_image = np.zeros([480, 640],
dtype=np.float32)
self.cur_head_camera_rgb_image = np.zeros([480, 640, 3],
dtype=np.uint8)
self.timeseq = None
self.timeseq_mutex = threading.Lock()
self.image_queue = Queue.Queue()
self.image_compress_thread = threading.Thread(
target=self.image_compress_main)
self.image_compress_thread.daemon = True
self.image_compress_thread.start()
logger.info('Subscribing...')
self.subs = []
if 1:
self.subs.append(
rospy.Subscriber('/joint_states', JointState,
self.joint_states_cb))
if 0:
self.subs.append(
rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
if 1:
self.subs.append(
rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image),
self.head_camera_depth_image_cb))
if 1:
self.subs.append(
rospy.Subscriber('/head_camera/rgb/image_raw',
numpy_msg(Image),
self.head_camera_rgb_image_raw_cb))
if 1:
self.subs.append(
rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
logger.info('Subscribed')
self.arm_effort_pub = rospy.Publisher(
'/arm_controller/weightless_torque/command',
JointTrajectory,
queue_size=2)
#self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
self.gripper_client = actionlib.SimpleActionClient(
'gripper_controller/gripper_action', GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher(
'/arm_controller/cartesian_twist/command', Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient(
'head_controller/point_head', PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm",
"base_link",
plan_only=True)
self.arm_trajectory_client = actionlib.SimpleActionClient(
"arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.arm_trajectory_client.wait_for_server()
if 0:
logger.info('Creating MoveGroupInterface...')
self.move_group = MoveGroupInterface('arm_with_torso',
'base_link',
plan_only=True)
logger.info('Created MoveGroupInterface')
if 0:
logger.info('Creating PlanningSceneInterface...')
self.planning_scene = PlanningSceneInterface('base_link')
self.planning_scene.removeCollisionObject('my_front_ground')
self.planning_scene.removeCollisionObject('my_back_ground')
self.planning_scene.removeCollisionObject('my_right_ground')
self.planning_scene.removeCollisionObject('my_left_ground')
self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0,
-1.0)
self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0,
-1.0)
self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2,
-1.0)
self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2,
-1.0)
logger.warn('FetchRobotGymEnv ROS node running')
self.head_point_client.wait_for_server()
logger.warn('FetchRobotGymEnv ROS node connected')
def start_timeseq(self, script):
timeseq = TimeseqWriter.create(script)
timeseq.add_channel('robot_joints', 'FetchRobotJoints')
timeseq.add_channel('gripper_joints', 'FetchGripperJoints')
timeseq.add_schema(
'FetchRobotJoints',
('torso_lift_joint', 'JointState'),
('head_pan_joint', 'JointState'),
('head_tilt_joint', 'JointState'),
('shoulder_pan_joint', 'JointState'),
('shoulder_lift_joint', 'JointState'),
('upperarm_roll_joint', 'JointState'),
('elbow_flex_joint', 'JointState'),
('forearm_roll_joint', 'JointState'),
('wrist_flex_joint', 'JointState'),
('wrist_roll_joint', 'JointState'),
)
timeseq.add_schema(
'FetchGripperJoints',
('l_gripper_finger_joint', 'JointState'),
)
timeseq.add_schema(
'JointState',
('pos', 'double'),
('vel', 'double'),
('effort', 'double'),
)
timeseq.add_channel('arm_action', 'FetchArmWeightlessTorqueAction')
timeseq.add_schema(
'FetchArmWeightlessTorqueAction',
('shoulder_pan_joint', 'WeightlessTorqueAction'),
('shoulder_lift_joint', 'WeightlessTorqueAction'),
('upperarm_roll_joint', 'WeightlessTorqueAction'),
('elbow_flex_joint', 'WeightlessTorqueAction'),
('forearm_roll_joint', 'WeightlessTorqueAction'),
('wrist_flex_joint', 'WeightlessTorqueAction'),
('wrist_roll_joint', 'WeightlessTorqueAction'),
)
timeseq.add_schema(
'WeightlessTorqueAction',
('action', 'double'),
('effort', 'double'),
)
timeseq.add_channel('gripper_action', 'FetchGripperAction')
timeseq.add_schema(
'FetchGripperAction',
('action', 'double'),
('effort', 'double'),
('pos', 'double'),
)
timeseq.add_channel('head_camera_rgb', 'VideoFrame')
timeseq.add_schema(
'VideoFrame',
('url', 'string'),
)
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.close()
self.timeseq = timeseq
def close_timeseq(self):
with self.timeseq_mutex:
if self.timeseq is not None:
self.timeseq.close()
threading.Thread(target=self.timeseq.upload_s3).start()
self.timeseq = None
def start_axis_video(self):
cameras = rospy.get_param('/axis_cameras')
if cameras and len(cameras):
with self.timeseq_mutex:
if self.timeseq:
for name, info in cameras.items():
logging.info('Camera %s: %s', name, repr(info))
self.timeseq.start_axis_video(
timeseq_name=name,
auth_header=info.get('auth_header'),
daemon_endpoint=info.get('daemon_endpoint'),
ipaddr=info.get('ipaddr'),
local_link_prefix=info.get('local_link_prefix'),
remote_traces_dir=info.get('remote_traces_dir'),
resolution=info.get('resolution'))
def stop_axis_video(self):
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.stop_axis_video()
def base_scan_cb(self, data):
# fmin replaces nans with 15
self.cur_base_scan = np.fmin(np.array(data.ranges), 15.0)
def head_camera_depth_image_cb(self, data):
shape = [data.height, data.width]
dtype = np.dtype(np.float32)
npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
npdata.strides = (data.step, dtype.itemsize)
self.cur_head_camera_depth_image = np.fmin(npdata, 5.0)
def head_camera_rgb_image_raw_cb(self, data):
shape = [data.height, data.width, 3]
dtype = np.dtype(np.uint8)
npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
npdata.strides = (data.step, dtype.itemsize * 3, 1)
self.cur_head_camera_rgb_image = npdata
if self.timeseq:
self.image_queue.put(('head_camera_rgb', data))
def joint_states_cb(self, data):
"""
Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping.
"""
t0 = time.time()
for i in range(len(data.name)):
name = data.name[i]
jni = self.joint_name_map.get(name, -1)
if jni >= 0:
self.cur_joint_pos[jni] = data.position[i]
self.cur_joint_vel[jni] = data.velocity[i]
self.cur_joint_effort[jni] = data.effort[i]
with self.timeseq_mutex:
if self.timeseq:
channel, channel_type = (
(data.name[0] == 'l_wheel_joint' and
('robot_joints', 'FetchRobotJoints'))
or (data.name[0] == 'l_gripper_finger_joint' and
('gripper_joints', 'FetchGripperJoints'))
or (None, None))
if channel:
state = dict([(jn, {
'__type': 'JointState',
'pos': data.position[i],
'vel': data.velocity[i],
'effort': data.effort[i],
}) for i, jn in enumerate(data.name)])
state['__type'] = channel_type
state['rxtime'] = t0
self.timeseq.add(data.header.stamp.to_sec(), channel,
state)
if 0:
t1 = time.time()
print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % (
t0, t1 - t0, data.header.stamp.to_sec(),
data.header.stamp.to_sec() - t0, channel)
def image_compress_main(self):
while True:
channel, data = self.image_queue.get()
if 0:
print 'len(data.data)=%d data.width=%d data.height=%d' % (len(
data.data), data.width, data.height)
im = PIL.Image.frombytes('RGB', (data.width, data.height),
data.data, 'raw')
jpeg_writer = StringIO()
im.save(jpeg_writer, 'jpeg')
im_url = 'data:image/jpeg;base64,' + base64.b64encode(
jpeg_writer.getvalue())
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.add(data.header.stamp.to_sec(), channel, {
'__type': 'VideoFrame',
'url': im_url,
})
def move_to_start(self):
self.moving_mode = True
# Look down
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = '/base_link'
goal.target.point.x = 1.5
goal.target.point.y = 0.0
goal.target.point.z = -0.2
goal.min_duration = rospy.Duration(0.5)
if 0: logger.info('Point head to %s...', goal)
self.head_point_client.send_goal(goal)
if 0: logger.info('Point head sent')
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
joints = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
]
pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
result = self.arm_move_group.moveToJointPosition(joints,
pose,
plan_only=True)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if 0: logger.info('Got trajectory %s', result.planned_trajectory)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = result.planned_trajectory.joint_trajectory
logger.info('sending trajectory to arm...')
self.arm_trajectory_client.send_goal(follow_goal)
result = self.arm_trajectory_client.wait_for_result()
logger.info('arm followed trajectory %s', result)
else:
logger.warn('moveToJointPosition returned %s', result)
return
result = self.head_point_client.wait_for_result()
logger.info('head followed trajectory %s', result)
logger.info('sending empty arm goal')
empty_goal = FollowJointTrajectoryGoal()
self.arm_trajectory_client.send_goal(empty_goal)
logger.info('Point head done')
self.moving_mode = False
def set_arm_action(self, action):
arm_joints = [
('shoulder_pan_joint', 1.57, 33.82),
('shoulder_lift_joint', 1.57, 131.76),
('upperarm_roll_joint', 1.57, 76.94),
('elbow_flex_joint', 1.57, 66.18),
('forearm_roll_joint', 1.57, 29.35),
('wrist_flex_joint', 2.26, 25.70),
('wrist_roll_joint', 2.26, 7.36),
]
arm_efforts = [
min(1.0, max(-1.0, action[self.joint_name_map.get(name)])) *
torque_scale * 0.75 for name, vel_scale, torque_scale in arm_joints
]
arm_joint_names = [
name for name, vel_scale, torque_scale in arm_joints
]
if 1:
arm_joint_names.append('gravity_compensation')
arm_efforts.append(1.0)
arm_msg = JointTrajectory(
joint_names=arm_joint_names,
points=[JointTrajectoryPoint(effort=arm_efforts)])
self.arm_effort_pub.publish(arm_msg)
with self.timeseq_mutex:
if self.timeseq:
state = dict([(jn, {
'__type': 'WeightlessTorqueAction',
'action': action[self.joint_name_map.get(jn)],
'effort': arm_efforts[i],
}) for i, jn in enumerate(arm_joint_names)])
state['__type'] = 'FetchArmWeightlessTorqueAction'
self.timeseq.add(time.time(), 'arm_action', state)
def set_gripper_action(self, action):
grip = action[self.joint_name_map.get('l_gripper_finger_joint')]
goal = GripperCommandGoal()
if grip > 0:
goal.command.max_effort = 60 * min(1.0, grip)
goal.command.position = 0.0
else:
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
with self.timeseq_mutex:
if self.timeseq:
state = {
'__type': 'FetchGripperAction',
'action': grip,
'effort': goal.command.max_effort,
'pos': goal.command.position,
}
self.timeseq.add(time.time(), 'gripper_action', state)
def set_waldo_action(self, joy):
twist = Twist()
twist.linear.x = joy.axes[0]
twist.linear.y = joy.axes[1]
twist.linear.z = joy.axes[2]
twist.angular.x = +3.0 * joy.axes[3]
twist.angular.y = +3.0 * joy.axes[4]
twist.angular.z = +2.0 * joy.axes[5]
self.arm_cartesian_twist_pub.publish(twist)
if joy.buttons[1] and not self.prev_joy_buttons[1]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.0
self.gripper_client.send_goal(goal)
elif not joy.buttons[1] and self.prev_joy_buttons[1]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
def spacenav_joy_cb(self, joy):
if 0: logger.info('joy %s', str(joy))
if joy.buttons[0] and not self.prev_joy_buttons[0]:
if self.waldo_mode:
self.stop_waldo_mode()
elif not self.moving_mode:
self.start_waldo_mode()
if self.waldo_mode and not self.moving_mode:
self.set_waldo_action(joy)
self.prev_joy_buttons = joy.buttons
def start_waldo_mode(self):
logger.info('Start waldo mode')
self.waldo_mode = True
self.start_timeseq('fetchwaldo')
self.start_axis_video()
def stop_waldo_mode(self):
logger.info('Stop waldo mode')
self.waldo_mode = False
timeseq_url = None
if self.timeseq:
timeseq_url = self.timeseq.get_url()
logger.info('save_logs url=%s', timeseq_url)
self.stop_axis_video()
self.close_timeseq()
mts_thread = threading.Thread(target=self.move_to_start)
mts_thread.daemon = True
mts_thread.start()
def picknplace():
# Define initial parameters
p = PlanningSceneInterface("base")
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
# jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
# jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
# jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
# pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
# pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
# lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
# rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
# rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# Clear planning scene
# p.clear()
# # Add table as attached object
# p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# Move both arms to start state
# g.moveToJointPosition(jts_both, pos1, plan_only=False)
# # Get obj locations
# temp = rospy.wait_for_message("Dpos", PoseArray)
# locs = temp.poses
# locs_x = []
# locs_y = []
# orien = []
# size = []
# for i in range(len(locs)):
# locs_x.append(0.57 + locs[i].position.x)
# locs_y.append(-0.011 + locs[i].position.y)
# orien.append(locs[i].position.z*pi/180)
# size.append(locs[i].orientation.x)
# # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
# ind_rmv = []
# for i in range(0,len(locs)):
# if (locs_y[i] > 0.42):
# ind_rmv.append(i)
# continue
# for j in range(i,len(locs)):
# if not (i == j):
# if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
# ind_rmv.append(i)
# locs_x = del_meth(locs_x, ind_rmv)
# locs_y = del_meth(locs_y, ind_rmv)
# orien = del_meth(orien, ind_rmv)
# size = del_meth(size, ind_rmv)
# # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
# if locs_x:
# ig0 = itemgetter(0)
# sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
# locs_x = list(sorted_lists[1])
# locs_y = list(sorted_lists[2])
# orien = list(sorted_lists[3])
# size = list(sorted_lists[0])
# # Loop to continue pick and place until all objects are cleared from table
# j=0
# k=0
# while locs_x:
# p.clear() # CLear planning scene of all collision objects
# # Attach table as attached collision object to base of baxter
# p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# xn = locs_x[0]
# yn = locs_y[0]
# zn = -0.06
# thn = orien[0]
# sz = size[0]
# if thn > pi/4:
# thn = -1*(thn%(pi/4))
# # Add collision objects into planning scene
# objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
# for i in range(1,len(locs_x)):
# p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
# p.waitForSync()
# # Move both arms to pos2 (Right arm away and left arm on table)
# g.moveToJointPosition(jts_both, pos2, plan_only=False)
# Move left arm to pick object and pick object
# pickgoal = PoseStamped()
# pickgoal.header.frame_id = "base"
# pickgoal.header.stamp = rospy.Time.now()
# pickgoal.pose.position.x = xn
# pickgoal.pose.position.y = yn
# pickgoal.pose.position.z = zn+0.1
# pickgoal.pose.orientation.x = 1.0
# pickgoal.pose.orientation.y = 0.0
# pickgoal.pose.orientation.z = 0.0
# pickgoal.pose.orientation.w = 0.0
goal_pose = PoseStamped()
limb = baxter_interface.Limb('left')
gripper_pose = limb.endpoint_pose()
goal_pose.pose.position.x = gripper_pose['position'][0] #0.63
goal_pose.pose.position.y = gripper_pose['position'][1] #0.19519
goal_pose.pose.position.z = gripper_pose['position'][2] + 0.1 #0.05
goal_pose.pose.orientation.x = gripper_pose['orientation'][0]
goal_pose.pose.orientation.y = gripper_pose['orientation'][1]
goal_pose.pose.orientation.z = gripper_pose['orientation'][2]
goal_pose.pose.orientation.w = gripper_pose['orientation'][3]
gl.moveToPose(goal_pose, "left_gripper", plan_only=False)
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m-1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene=self.scene)
return success
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def main():
rospy.init_node("arm_obstacle_demo")
wait_for_time()
argv = rospy.myargv()
planning_scene = PlanningSceneInterface("base_link")
# Create table obstacle
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
# Create divider obstacle
planning_scene.removeCollisionObject('divider')
size_x = 0.3
size_y = 0.01
size_z = 0.2
x = table_x - (table_size_x / 2) + (size_x / 2)
y = 0
z = table_z + (table_size_z / 2) + (size_z / 2)
planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
gripper = robot_api.Gripper()
arm = robot_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
kwargs = {
'allowed_planning_time': 15,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': True,
'orientation_constraint': oc
}
planning_scene.removeAttachedObject('tray')
gripper.open()
error = arm.move_to_pose(pose1, **kwargs)
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
rospy.sleep(2)
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to, frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
gripper.close()
rospy.sleep(2)
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
gripper.open()
planning_scene.removeAttachedObject('tray')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
return
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
# here i am declaring goal as an object. https://github.com/mikeferguson/grasping_msgs/blob/master/action/FindGraspableObjects.action
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
# passing the object to find_client
# now find_client is wer magic happens
# on demo.launch file i am runnning basic_grasping_perception. (below <!-- Start Perception -->)
# this keeps running on background and i use actionlib (initalize on init) to get a hook to it.
# find_client is connected to basic_grasping_perception
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
# here we get all the objects
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
rospy.loginfo("updating scene")
idx = 0
# insert objects to the planning scene
#TODO so these two for loops yo can hardcode the values. try printing all the params and u will understand
for obj in find_result.objects:
rospy.loginfo("object number -> %d" %idx)
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
idx += 1
# for grp in obj.grasps:
# grp.grasp_pose.pose.position.z = 0.37
# this is just minor adjustments i did mess up with this code. just follwed simple gasp thingy
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
rospy.loginfo("height before => %f" % height)
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
rospy.loginfo("height after => %f" % obj.primitive_poses[0].position.z)
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
rospy.loginfo("no. of grasps detected %d dim => %f" % (len(obj.grasps), obj.object.primitives[0].dimensions[0]))
if len(obj.grasps) < 1:
continue
# check size our object is a 0.05^3 cube
if obj.object.primitives[0].dimensions[0] < 0.04 or \
obj.object.primitives[0].dimensions[0] > 0.06 or \
obj.object.primitives[0].dimensions[1] < 0.04 or \
obj.object.primitives[0].dimensions[1] > 0.06 or \
obj.object.primitives[0].dimensions[2] < 0.04 or \
obj.object.primitives[0].dimensions[2] > 0.06:
continue
rospy.loginfo("###### size: x => %f, y => %f, z => %f" % (obj.object.primitive_poses[0].position.x,obj.object.primitive_poses[0].position.y,obj.object.primitive_poses[0].position.z))
# has to be on table
#if obj.object.primitive_poses[0].position.z < 0.5:grasping_client
# continue
return obj.object, obj.grasps
# nothing detected
rospy.loginfo("nothing detected")
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m-1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene=self.scene)
return success
def tuck(self):
# so for each joint i will pass a specific value. first arms are move to pose1 then pose2 then pose3 sequentially
joints = ["shoulder_roll_joint", "shoulder_pitch_joint", "shoulder_yaw_joint",
"elbow_pitch_joint", "elbow_yaw_joint", "wrist_pitch_joint", "wrist_roll_joint"]
pose1 = [1.57079633, 0, 0, 0, 0, 0, 0.0]
pose2 = [1.57079633, 0, -1.57079633, 0, -1.57079633, 0, 0.0]
pose3 = [1.57079633, 1.57079633, -1.57079633, 0, -1.57079633, 1.57079633, 0.0]
while not rospy.is_shutdown():
self.move_group.moveToJointPosition(joints, pose1, 0.02)
self.move_group.moveToJointPosition(joints, pose2, 0.02)
result = self.move_group.moveToJointPosition(joints, pose3, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class World:
def __init__(self, tower_array, disk_array, config, debug=0):
print "[INFO] Initialise World"
self.DEBUG = debug
# initialise arrays
self.tower_array = tower_array
self.disk_array = disk_array
# configs
self.max_gripper = config.get_max_gripper()
self.disk_height = config.disk_height
self.tower_positions = config.tower_positions
self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
"""Calls a method to display the collision objects.
"""
def create_planning_scene(self):
print "[INFO] Create_planning_scene"
self.display_towers_and_disks()
"""This method collects all needed information and
publish them to other methods.
"""
def display_towers_and_disks(self):
print "[INFO] Display towers and disks"
for tower in self.tower_array:
# call method to publish new tower
self.publish_scene(tower.get_position(), tower)
# set color by name
self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0,
1.0)
# publish color
self.planning_scene_interface.sendColors()
for disk in self.disk_array:
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(),
disk.get_color()[0],
disk.get_color()[1],
disk.get_color()[2],
disk.get_color()[3])
self.planning_scene_interface.sendColors()
# wait for sync after publishing collision objects
self.planning_scene_interface.waitForSync(5.0)
rospy.sleep(5)
"""This method creates a box or a cylinder with methods of
the planning scene interface.
:param position: the position of the new collision object
:param tower: the tower object
:param disk: the disk object
"""
def publish_scene(self, position, tower=None, disk=None):
if tower is not None:
self.planning_scene_interface.addBox(
tower.get_name(), self.max_gripper / 100.0,
self.max_gripper / 100.0,
(self.tower_positions[tower.get_id() - 1][2] * 2), position[0],
position[1], position[2])
else:
self.planning_scene_interface.addCylinder(disk.get_id(),
self.disk_height,
disk.get_diameter() / 2,
position[0], position[1],
position[2])
"""This method cleans the planning scene.
:param close: with this flag a new planning scene objects will be removed
in sync otherwise the object will be deleted without syncing the scene
"""
def clean_up(self, close=False):
if close:
# get the actual list after game
self.planning_scene_interface = PlanningSceneInterface(
REFERENCE_FRAME)
for name in self.planning_scene_interface.getKnownCollisionObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing object %s" % name)
self.planning_scene_interface.removeCollisionObject(name, False)
for name in self.planning_scene_interface.getKnownAttachedObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing attached object %s" % name)
self.planning_scene_interface.removeAttachedObject(name, False)
if close:
self.planning_scene_interface.waitForSync(5.0)
pass
"""This method corrects the position of the moved disk.
:param tower: parent tower of the disk
"""
def refresh_disk_pose(self, tower):
print "[INFO] Refresh disk pose"
disk = tower.get_last()
if self.DEBUG is 1:
print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\
"tower pose", tower.get_position()
# remove the disk from planning scene
self.planning_scene_interface.removeCollisionObject(
disk.get_id(), False)
# publish collision object and set old color
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(),
disk.get_color()[0],
disk.get_color()[1],
disk.get_color()[2],
disk.get_color()[3])
self.planning_scene_interface.sendColors()
def __init__(self):
self.moving_mode = False
self.waldo_mode = False
self.prev_joy_buttons = 0
# See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
self.joint_names = [
#'torso_lift_joint',
#'bellows_joint',
#'head_pan_joint',
#'head_tilt_joint',
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
'l_gripper_finger_joint',
#'r_gripper_finger_joint',
]
self.joint_name_map = dict([(name, index) for index, name in enumerate(self.joint_names)])
self.cur_joint_pos = np.zeros([len(self.joint_names)])
self.cur_joint_vel = np.zeros([len(self.joint_names)])
self.cur_joint_effort = np.zeros([len(self.joint_names)])
self.cur_base_scan = np.zeros([662], dtype=np.float32)
self.cur_head_camera_depth_image = np.zeros([480,640], dtype=np.float32)
self.cur_head_camera_rgb_image = np.zeros([480,640,3], dtype=np.uint8)
self.timeseq = None
self.timeseq_mutex = threading.Lock()
self.image_queue = Queue.Queue()
self.image_compress_thread = threading.Thread(target=self.image_compress_main)
self.image_compress_thread.daemon = True
self.image_compress_thread.start()
logger.info('Subscribing...')
self.subs = []
if 1: self.subs.append(rospy.Subscriber('/joint_states', JointState, self.joint_states_cb))
if 0: self.subs.append(rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
if 1: self.subs.append(rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image), self.head_camera_depth_image_cb))
if 1: self.subs.append(rospy.Subscriber('/head_camera/rgb/image_raw', numpy_msg(Image), self.head_camera_rgb_image_raw_cb))
if 1: self.subs.append(rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
logger.info('Subscribed')
self.arm_effort_pub = rospy.Publisher('/arm_controller/weightless_torque/command', JointTrajectory, queue_size=2)
#self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
self.gripper_client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher('/arm_controller/cartesian_twist/command', Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True)
self.arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
self.arm_trajectory_client.wait_for_server()
if 0:
logger.info('Creating MoveGroupInterface...')
self.move_group = MoveGroupInterface('arm_with_torso', 'base_link', plan_only = True)
logger.info('Created MoveGroupInterface')
if 0:
logger.info('Creating PlanningSceneInterface...')
self.planning_scene = PlanningSceneInterface('base_link')
self.planning_scene.removeCollisionObject('my_front_ground')
self.planning_scene.removeCollisionObject('my_back_ground')
self.planning_scene.removeCollisionObject('my_right_ground')
self.planning_scene.removeCollisionObject('my_left_ground')
self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2, -1.0)
logger.warn('FetchRobotGymEnv ROS node running')
self.head_point_client.wait_for_server()
logger.warn('FetchRobotGymEnv ROS node connected')
class GripperInterfaceClient:
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
rospy.loginfo("...connected.")
self.scene = PlanningSceneInterface("base_link")
self.attached_object_publisher = rospy.Publisher(
"attached_collision_object", AttachedCollisionObject, queue_size=10
)
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
self.debug_index = -1
self.graspables = []
def execute_trajectory(self, trajectory):
goal_position = trajectory.points[-1].positions
print "==========", goal_position
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position[0] + goal_position[1]
command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this
self.client.send_goal(command_goal)
self.client.wait_for_result()
def move_to(self, goal_position, max_effort):
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position
command_goal.command.max_effort = max_effort
self.client.send_goal(command_goal)
self.client.wait_for_result()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, wait=False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, wait=False)
rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition.
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(
obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False
)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0] + 0.1,
1.5, # wider
obj.primitives[0].dimensions[2] + height,
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
# if obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07 or \
# obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07 or \
# obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07:
# continue
# has to be on table
# if obj.object.primitive_poses[0].position.z < 0.5:
# continue
return obj.object, obj.grasps
# nothing detected
return None, None
def set_target_object(self, primitive_type, target_pose, dimensions):
primitive_types = {"BOX": 1, "SPHERE": 2, "CYLINDER": 3, "CONE": 4}
self.sought_object = Object()
primitive = SolidPrimitive()
primitive.type = primitive_types[primitive_type.upper()]
primitive.dimensions = dimensions
self.sought_object.primitives.append(primitive)
p = Pose()
p.position.x = target_pose[0]
p.position.y = target_pose[1]
p.position.z = target_pose[2]
quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5])
p.orientation.x = quaternion[0]
p.orientation.y = quaternion[1]
p.orientation.z = quaternion[2]
p.orientation.w = quaternion[3]
self.sought_object.primitive_poses.append(p)
def find_graspable_object(self, tolerance=0.01):
self.updateScene()
self.current_grasping_target = None
for obj in self.objects:
# Must have grasps
if len(obj.grasps) < 1:
continue
# Compare to sought object
detected_object_dimensions = np.array(obj.object.primitives[0].dimensions)
sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions)
try:
difference = detected_object_dimensions - sought_object_dimensions
print "===DEBUG: Difference: ", difference
mag_diff = np.linalg.norm(difference)
print "===DEBUG: mag_diff: ", mag_diff
if mag_diff <= tolerance:
self.current_grasping_target = obj
tolerance = mag_diff
print "===DEBUG: Tolerance: ", tolerance
else:
rospy.loginfo("Object dimensions do not match. Trying another object...")
except:
rospy.loginfo("Object geometries do not match. Trying another object...")
if self.current_grasping_target is None:
tolerance += 0.01
self.find_graspable_object(tolerance=tolerance)
# Nothing detected
def computeGraspToPickMatrix(self):
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion)
translation = [pick_translation_distance, 0, 0]
rotation_matrix[[0, 1, 2], 3] = translation
pick_to_grasp_matrix = np.mat(rotation_matrix)
grasp_to_pick_matrix = pick_to_grasp_matrix.getI()
return grasp_to_pick_matrix
def computePlaceToBaseMatrix(self, place):
place_quaternion = place[3:]
rotation_matrix = quaternion_matrix(place_quaternion)
translation = place[0:3]
rotation_matrix[[0, 1, 2], 3] = translation
place_to_base_matrix = rotation_matrix
return place_to_base_matrix
def broadcastCurrentGraspingTargetTransform(self):
pose = self.current_grasping_target.object.primitive_poses[0]
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "base_link"
t.child_frame_id = "current_grasping_target"
t.transform.translation.x = pose.position.x
t.transform.translation.y = pose.position.y
t.transform.translation.z = pose.position.z
t.transform.rotation.x = pose.orientation.x
t.transform.rotation.y = pose.orientation.y
t.transform.rotation.z = pose.orientation.z
t.transform.rotation.w = pose.orientation.w
br.sendTransform(t)
def plan_pick(self):
success, pick_result = self.pickplace.pick_with_retry(
self.current_grasping_target.object.name,
self.current_grasping_target.grasps,
support_name=self.current_grasping_target.object.support_surface,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id="PRMkConfigDefault",
)
self.pick_result = pick_result
print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects()
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return pick_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Getting new grasps for replan...")
else:
rospy.logwarn("Planning failed. Getting new grasps for replan...")
self.find_graspable_object()
return self.plan_pick()
def plan_place(self, desired_pose):
places = list()
ps = PoseStamped()
# ps.pose = self.current_grasping_target.object.primitive_poses[0]
ps.header.frame_id = self.current_grasping_target.object.header.frame_id
grasp_to_pick_matrix = self.computeGraspToPickMatrix()
place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose)
grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix
position_vector = grasp2_to_place_matrix[0:-1, 3]
quaternion = quaternion_from_matrix(grasp2_to_place_matrix)
position_array = position_vector.getA1()
l = PlaceLocation()
direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector
l.place_pose.header.frame_id = ps.header.frame_id
l.place_pose.pose.position.x = position_array[0]
l.place_pose.pose.position.y = position_array[1]
l.place_pose.pose.position.z = position_array[2]
l.place_pose.pose.orientation.x = quaternion[0]
l.place_pose.pose.orientation.y = quaternion[1]
l.place_pose.pose.orientation.z = quaternion[2]
l.place_pose.pose.orientation.w = quaternion[3]
# copy the posture, approach and retreat from the grasp used
approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach)
approach.desired_distance /= 2.0
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 90 degrees in roll direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
print "DEBUG: Places: ", places[0]
success, place_result = self.pickplace.place_with_retry(
self.current_grasping_target.object.name,
places,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id="PRMkConfigDefault",
)
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return place_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Replanning...")
else:
rospy.logwarn("Planning failed. Replanning...")
desired_pose[2] += 0.05
return self.plan_place(desired_pose)
def recognizeAttachedObject(self):
print "========= Recognizing attached object"
name = self.current_grasping_target.object.name
size_x = self.current_grasping_target.object.primitives[0].dimensions[0]
size_y = self.current_grasping_target.object.primitives[0].dimensions[1]
size_z = self.current_grasping_target.object.primitives[0].dimensions[2]
(x, y, z) = (0.03, 0.0, 0.0)
link_name = "gripper_link"
touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"]
detach_posture = None
weight = 0.0
wait = True
"""
object_x = self.current_grasping_target.object.primitive_poses[0].position.x
object_y = self.current_grasping_target.object.primitive_poses[0].position.y
object_z = self.current_grasping_target.object.primitive_poses[0].position.z
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
grasp_position = self.pick_result.grasp.grasp_pose.pose.position
pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion)
grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array)
displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]]))
print displacement
attached_location = list()
attached_location.append(object_x - displacement[0,0])
attached_location.append(object_y - displacement[1,0])
attached_location.append(object_z - displacement[2,0])
print attached_location
quaternion = Quaternion()
quaternion.x = pick_to_grasp_quaternion[0]
quaternion.y = pick_to_grasp_quaternion[1]
quaternion.z = pick_to_grasp_quaternion[2]
quaternion.w = pick_to_grasp_quaternion[3]
pose = Pose()
pose.position.x = attached_location[0]
pose.position.y = attached_location[1]
pose.position.z = attached_location[2]
pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation
collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name,
self.current_grasping_target.object.primitives[0],
pose)
attached_collision_object = self.scene.makeAttached("gripper_link",
collision_object,
touch_links, detach_posture, 0.0)
self.attached_object_publisher.publish(attached_collision_object)
"""
self.scene.attachBox(
name,
size_x,
size_y,
size_z,
x,
y,
z,
link_name,
touch_links=touch_links,
detach_posture=detach_posture,
weight=weight,
wait=wait,
)
def removeCollisionObjects(self):
collision_object_names = self.scene.getKnownCollisionObjects()
print self.current_grasping_target.object.name
print collision_object_names
raw_input("press ENTER")
x = self.scene.getKnownAttachedObjects()
print x
raw_input("press Enter")
for name in collision_object_names:
if name != self.current_grasping_target.object.name:
self.scene.removeCollisionObject(name, wait=False)
class FetchRobotApi:
def __init__(self):
self.moving_mode = False
self.waldo_mode = False
self.prev_joy_buttons = 0
# See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
self.joint_names = [
#'torso_lift_joint',
#'bellows_joint',
#'head_pan_joint',
#'head_tilt_joint',
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
'l_gripper_finger_joint',
#'r_gripper_finger_joint',
]
self.joint_name_map = dict([(name, index) for index, name in enumerate(self.joint_names)])
self.cur_joint_pos = np.zeros([len(self.joint_names)])
self.cur_joint_vel = np.zeros([len(self.joint_names)])
self.cur_joint_effort = np.zeros([len(self.joint_names)])
self.cur_base_scan = np.zeros([662], dtype=np.float32)
self.cur_head_camera_depth_image = np.zeros([480,640], dtype=np.float32)
self.cur_head_camera_rgb_image = np.zeros([480,640,3], dtype=np.uint8)
self.timeseq = None
self.timeseq_mutex = threading.Lock()
self.image_queue = Queue.Queue()
self.image_compress_thread = threading.Thread(target=self.image_compress_main)
self.image_compress_thread.daemon = True
self.image_compress_thread.start()
logger.info('Subscribing...')
self.subs = []
if 1: self.subs.append(rospy.Subscriber('/joint_states', JointState, self.joint_states_cb))
if 0: self.subs.append(rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
if 1: self.subs.append(rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image), self.head_camera_depth_image_cb))
if 1: self.subs.append(rospy.Subscriber('/head_camera/rgb/image_raw', numpy_msg(Image), self.head_camera_rgb_image_raw_cb))
if 1: self.subs.append(rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
logger.info('Subscribed')
self.arm_effort_pub = rospy.Publisher('/arm_controller/weightless_torque/command', JointTrajectory, queue_size=2)
#self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
self.gripper_client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher('/arm_controller/cartesian_twist/command', Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True)
self.arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
self.arm_trajectory_client.wait_for_server()
if 0:
logger.info('Creating MoveGroupInterface...')
self.move_group = MoveGroupInterface('arm_with_torso', 'base_link', plan_only = True)
logger.info('Created MoveGroupInterface')
if 0:
logger.info('Creating PlanningSceneInterface...')
self.planning_scene = PlanningSceneInterface('base_link')
self.planning_scene.removeCollisionObject('my_front_ground')
self.planning_scene.removeCollisionObject('my_back_ground')
self.planning_scene.removeCollisionObject('my_right_ground')
self.planning_scene.removeCollisionObject('my_left_ground')
self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2, -1.0)
logger.warn('FetchRobotGymEnv ROS node running')
self.head_point_client.wait_for_server()
logger.warn('FetchRobotGymEnv ROS node connected')
def start_timeseq(self, script):
timeseq = TimeseqWriter.create(script)
timeseq.add_channel('robot_joints', 'FetchRobotJoints')
timeseq.add_channel('gripper_joints', 'FetchGripperJoints')
timeseq.add_schema('FetchRobotJoints',
('torso_lift_joint', 'JointState'),
('head_pan_joint', 'JointState'),
('head_tilt_joint', 'JointState'),
('shoulder_pan_joint', 'JointState'),
('shoulder_lift_joint', 'JointState'),
('upperarm_roll_joint', 'JointState'),
('elbow_flex_joint', 'JointState'),
('forearm_roll_joint', 'JointState'),
('wrist_flex_joint', 'JointState'),
('wrist_roll_joint', 'JointState'),
)
timeseq.add_schema('FetchGripperJoints',
('l_gripper_finger_joint', 'JointState'),
)
timeseq.add_schema('JointState',
('pos', 'double'),
('vel', 'double'),
('effort', 'double'),
)
timeseq.add_channel('arm_action', 'FetchArmWeightlessTorqueAction')
timeseq.add_schema('FetchArmWeightlessTorqueAction',
('shoulder_pan_joint', 'WeightlessTorqueAction'),
('shoulder_lift_joint', 'WeightlessTorqueAction'),
('upperarm_roll_joint', 'WeightlessTorqueAction'),
('elbow_flex_joint', 'WeightlessTorqueAction'),
('forearm_roll_joint', 'WeightlessTorqueAction'),
('wrist_flex_joint', 'WeightlessTorqueAction'),
('wrist_roll_joint', 'WeightlessTorqueAction'),
)
timeseq.add_schema('WeightlessTorqueAction',
('action', 'double'),
('effort', 'double'),
)
timeseq.add_channel('gripper_action', 'FetchGripperAction')
timeseq.add_schema('FetchGripperAction',
('action', 'double'),
('effort', 'double'),
('pos', 'double'),
)
timeseq.add_channel('head_camera_rgb', 'VideoFrame')
timeseq.add_schema('VideoFrame',
('url', 'string'),
)
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.close()
self.timeseq = timeseq
def close_timeseq(self):
with self.timeseq_mutex:
if self.timeseq is not None:
self.timeseq.close()
threading.Thread(target=self.timeseq.upload_s3).start()
self.timeseq = None
def start_axis_video(self):
cameras = rospy.get_param('/axis_cameras')
if cameras and len(cameras):
with self.timeseq_mutex:
if self.timeseq:
for name, info in cameras.items():
logging.info('Camera %s: %s', name, repr(info))
self.timeseq.start_axis_video(timeseq_name = name,
auth_header=info.get('auth_header'),
daemon_endpoint=info.get('daemon_endpoint'),
ipaddr=info.get('ipaddr'),
local_link_prefix=info.get('local_link_prefix'),
remote_traces_dir=info.get('remote_traces_dir'),
resolution=info.get('resolution'))
def stop_axis_video(self):
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.stop_axis_video()
def base_scan_cb(self, data):
# fmin replaces nans with 15
self.cur_base_scan = np.fmin(np.array(data.ranges), 15.0)
def head_camera_depth_image_cb(self, data):
shape = [data.height, data.width]
dtype = np.dtype(np.float32)
npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
npdata.strides = (data.step, dtype.itemsize)
self.cur_head_camera_depth_image = np.fmin(npdata, 5.0)
def head_camera_rgb_image_raw_cb(self, data):
shape = [data.height, data.width, 3]
dtype = np.dtype(np.uint8)
npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
npdata.strides = (data.step, dtype.itemsize*3, 1)
self.cur_head_camera_rgb_image = npdata
if self.timeseq:
self.image_queue.put(('head_camera_rgb', data))
def joint_states_cb(self, data):
"""
Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping.
"""
t0 = time.time()
for i in range(len(data.name)):
name = data.name[i]
jni = self.joint_name_map.get(name, -1)
if jni >= 0:
self.cur_joint_pos[jni] = data.position[i]
self.cur_joint_vel[jni] = data.velocity[i]
self.cur_joint_effort[jni] = data.effort[i]
with self.timeseq_mutex:
if self.timeseq:
channel, channel_type = ((data.name[0] == 'l_wheel_joint' and ('robot_joints', 'FetchRobotJoints')) or
(data.name[0] == 'l_gripper_finger_joint' and ('gripper_joints', 'FetchGripperJoints')) or
(None, None))
if channel:
state = dict([(jn, {
'__type': 'JointState',
'pos': data.position[i],
'vel': data.velocity[i],
'effort': data.effort[i],
}) for i, jn in enumerate(data.name)])
state['__type'] = channel_type
state['rxtime'] = t0
self.timeseq.add(data.header.stamp.to_sec(), channel, state)
if 0:
t1 = time.time()
print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % (t0, t1-t0, data.header.stamp.to_sec(), data.header.stamp.to_sec()-t0, channel)
def image_compress_main(self):
while True:
channel, data = self.image_queue.get()
if 0: print 'len(data.data)=%d data.width=%d data.height=%d' % (len(data.data), data.width, data.height)
im = PIL.Image.frombytes('RGB', (data.width, data.height), data.data, 'raw')
jpeg_writer = StringIO()
im.save(jpeg_writer, 'jpeg')
im_url = 'data:image/jpeg;base64,' + base64.b64encode(jpeg_writer.getvalue())
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.add(data.header.stamp.to_sec(), channel, {
'__type': 'VideoFrame',
'url': im_url,
})
def move_to_start(self):
self.moving_mode = True
# Look down
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = '/base_link'
goal.target.point.x = 1.5
goal.target.point.y = 0.0
goal.target.point.z = -0.2
goal.min_duration = rospy.Duration(0.5)
if 0: logger.info('Point head to %s...', goal);
self.head_point_client.send_goal(goal)
if 0: logger.info('Point head sent')
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
joints = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
]
pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if 0: logger.info('Got trajectory %s', result.planned_trajectory)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = result.planned_trajectory.joint_trajectory
logger.info('sending trajectory to arm...')
self.arm_trajectory_client.send_goal(follow_goal)
result = self.arm_trajectory_client.wait_for_result()
logger.info('arm followed trajectory %s', result)
else:
logger.warn('moveToJointPosition returned %s', result)
return
result = self.head_point_client.wait_for_result()
logger.info('head followed trajectory %s', result)
logger.info('sending empty arm goal')
empty_goal = FollowJointTrajectoryGoal()
self.arm_trajectory_client.send_goal(empty_goal)
logger.info('Point head done')
self.moving_mode = False
def set_arm_action(self, action):
arm_joints = [
('shoulder_pan_joint', 1.57, 33.82),
('shoulder_lift_joint', 1.57, 131.76),
('upperarm_roll_joint', 1.57, 76.94),
('elbow_flex_joint', 1.57, 66.18),
('forearm_roll_joint', 1.57, 29.35),
('wrist_flex_joint', 2.26, 25.70),
('wrist_roll_joint', 2.26, 7.36),
]
arm_efforts = [min(1.0, max(-1.0, action[self.joint_name_map.get(name)])) * torque_scale * 0.75 for name, vel_scale, torque_scale in arm_joints]
arm_joint_names = [name for name, vel_scale, torque_scale in arm_joints]
if 1:
arm_joint_names.append('gravity_compensation')
arm_efforts.append(1.0)
arm_msg = JointTrajectory(joint_names=arm_joint_names, points=[JointTrajectoryPoint(effort = arm_efforts)])
self.arm_effort_pub.publish(arm_msg)
with self.timeseq_mutex:
if self.timeseq:
state = dict([(jn, {
'__type': 'WeightlessTorqueAction',
'action': action[self.joint_name_map.get(jn)],
'effort': arm_efforts[i],
}) for i, jn in enumerate(arm_joint_names)])
state['__type'] = 'FetchArmWeightlessTorqueAction'
self.timeseq.add(time.time(), 'arm_action', state)
def set_gripper_action(self, action):
grip = action[self.joint_name_map.get('l_gripper_finger_joint')]
goal = GripperCommandGoal()
if grip > 0:
goal.command.max_effort = 60*min(1.0, grip)
goal.command.position = 0.0
else:
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
with self.timeseq_mutex:
if self.timeseq:
state = {
'__type': 'FetchGripperAction',
'action': grip,
'effort': goal.command.max_effort,
'pos': goal.command.position,
}
self.timeseq.add(time.time(), 'gripper_action', state)
def set_waldo_action(self, joy):
twist = Twist()
twist.linear.x = joy.axes[0]
twist.linear.y = joy.axes[1]
twist.linear.z = joy.axes[2]
twist.angular.x = +3.0*joy.axes[3]
twist.angular.y = +3.0*joy.axes[4]
twist.angular.z = +2.0*joy.axes[5]
self.arm_cartesian_twist_pub.publish(twist)
if joy.buttons[1] and not self.prev_joy_buttons[1]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.0
self.gripper_client.send_goal(goal)
elif not joy.buttons[1] and self.prev_joy_buttons[1]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
def spacenav_joy_cb(self, joy):
if 0: logger.info('joy %s', str(joy))
if joy.buttons[0] and not self.prev_joy_buttons[0]:
if self.waldo_mode:
self.stop_waldo_mode()
elif not self.moving_mode:
self.start_waldo_mode()
if self.waldo_mode and not self.moving_mode:
self.set_waldo_action(joy)
self.prev_joy_buttons = joy.buttons
def start_waldo_mode(self):
logger.info('Start waldo mode');
self.waldo_mode = True
self.start_timeseq('fetchwaldo')
self.start_axis_video()
def stop_waldo_mode(self):
logger.info('Stop waldo mode');
self.waldo_mode = False
timeseq_url = None
if self.timeseq:
timeseq_url = self.timeseq.get_url()
logger.info('save_logs url=%s', timeseq_url)
self.stop_axis_video()
self.close_timeseq()
mts_thread = threading.Thread(target=self.move_to_start)
mts_thread.daemon = True
mts_thread.start()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface("base_link")
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(15)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Connect to the UBR-1 find_objects action server
rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
find_objects.wait_for_server()
rospy.loginfo("...connected")
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Begin the main perception and pick-and-place loop
while not rospy.is_shutdown():
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
# We don't use the UBR-1 grasp planner as it does not work with our gripper
goal.plan_grasps = False
# Send the goal request to the find_objects action server which will trigger
# the perception pipeline
find_objects.send_goal(goal)
# Wait for a result
find_objects.wait_for_result(rospy.Duration(5.0))
# The result will contain support surface(s) and objects(s) if any are detected
find_result = find_objects.get_result()
# Display the number of objects found
rospy.loginfo("Found %d objects" % len(find_result.objects))
# Remove all previous objects from the planning scene
for name in scene.getKnownCollisionObjects():
scene.removeCollisionObject(name, False)
for name in scene.getKnownAttachedObjects():
scene.removeAttachedObject(name, False)
scene.waitForSync()
# Clear the virtual object colors
scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_size = None
the_object = None
the_object_dist = 1.0
count = -1
# Cycle through all detected objects and keep the nearest one
for obj in find_result.objects:
count += 1
scene.addSolidPrimitive("object%d"%count,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
# Choose the object nearest to the robot
dx = obj.object.primitive_poses[0].position.x - args.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist:
the_object_dist = d
the_object = count
# Get the size of the target
target_size = obj.object.primitives[0].dimensions
# Set the target pose
target_pose.pose = obj.object.primitive_poses[0]
# We want the gripper to be horizontal
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Make sure we found at least one object before setting the target ID
if the_object != None:
target_id = "object%d"%the_object
# Insert the support surface into the planning scene
for obj in find_result.support_surfaces:
# Extend surface to floor
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
2.0, # make table wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# Add to scene
scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
# Get the table dimensions
table_size = obj.primitives[0].dimensions
# If no objects detected, try again
if the_object == None or target_size is None:
rospy.logerr("Nothing to grasp! try again...")
continue
# Wait for the scene to sync
scene.waitForSync()
# Set colors of the table and the object we are grabbing
scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange
scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey
scene.sendColors()
# Skip pick-and-place if we are just detecting objects
if args.objects:
if args.once:
exit(0)
else:
continue
# Get the support surface ID
support_surface = find_result.objects[the_object].object.support_surface
# Set the support surface name to the table object
right_arm.set_support_surface_name(support_surface)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = target_pose.pose.position.x
place_pose.pose.position.y = 0.03
place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose half the size of the target to center it in the gripper
try:
grasp_pose.pose.position.x += target_size[0] / 2.0
grasp_pose.pose.position.y -= 0.01
grasp_pose.pose.position.z += target_size[2] / 2.0
except:
rospy.loginfo("Invalid object size so skipping")
continue
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = right_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Set the start state to the current state
#right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
rospy.sleep(2)
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
# Give the servos a rest
arbotix_relax_all_servos()
rospy.sleep(2)
if args.once:
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import argparse
import rospy
from moveit_python import PlanningSceneInterface
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Attach objects to link in the MoveIt planning scene.")
parser.add_argument("name", help="Name of the box to be attached")
parser.add_argument("link", help="Name of link to where the box should be attached")
parser.add_argument("x", type=float, help="X coordinate of center of box")
parser.add_argument("y", type=float, help="Y coordinate of center of box")
parser.add_argument("z", type=float, help="Z coordinate of center of box")
parser.add_argument("size_x", type=float, help="Size of box in x dimension")
parser.add_argument("size_y", type=float, help="Size of box in y dimension")
parser.add_argument("size_z", type=float, help="Size of box in z dimension")
args = parser.parse_args()
if args.name:
if args.link:
rospy.init_node("attach_box")
scene = PlanningSceneInterface("/base_link")
print("Attach Object with name: %s to Link: %s" % (args.name, args.link))
scene.attachBox(args.name, args.size_x, args.size_y, args.size_z,
args.x, args.y, args.z, args.link)
else:
parser.print_help()
class PickPlaceDemo():
def __init__(self):
self._scene = PlanningSceneInterface('base_link')
self._pickplace = PickPlaceInterface('xarm5', 'gripper', verbose=True)
self._move_group = MoveGroupInterface('xarm5', 'base_link')
def setup_scene(self):
# remove previous objects
for name in self._scene.getKnownCollisionObjects():
self._scene.removeCollisionObject(name, False)
for name in self._scene.getKnownAttachedObjects():
self._scene.removeAttachedObject(name, False)
self._scene.waitForSync()
self.__addBoxWithOrientation('box',
0.25,
0.25,
0.25,
-0.45,
0.1,
0.125,
0,
0,
np.radians(45),
wait=False)
self._scene.waitForSync()
def __addBoxWithOrientation(self,
name,
size_x,
size_y,
size_z,
x,
y,
z,
roll,
pitch,
yaw,
wait=True):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
ps = PoseStamped()
ps.header.frame_id = self._scene._fixed_frame
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
ps.pose.orientation.x = q[0]
ps.pose.orientation.y = q[1]
ps.pose.orientation.z = q[2]
ps.pose.orientation.w = q[3]
self._scene.addSolidPrimitive(name, s, ps.pose, wait)
def pickup(self):
g = Grasp()
self._pickplace.pickup("box", [g], support_name="box")
def picknplace():
# Define initial parameters
p = PlanningSceneInterface("base")
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# Clear planning scene
p.clear()
# Add table as attached object
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# Move both arms to start state
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Loop to continue pick and place until all objects are cleared from table
j=0
k=0
while locs_x:
p.clear() # CLear planning scene of all collision objects
# Attach table as attached collision object to base of baxter
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
xn = locs_x[0]
yn = locs_y[0]
zn = -0.06
thn = orien[0]
sz = size[0]
if thn > pi/4:
thn = -1*(thn%(pi/4))
# Add collision objects into planning scene
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
for i in range(1,len(locs_x)):
p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
p.waitForSync()
# Move both arms to pos2 (Right arm away and left arm on table)
g.moveToJointPosition(jts_both, pos2, plan_only=False)
# Move left arm to pick object and pick object
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn+0.1
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles
pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
pickgoal.pose.position.z = zn
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
leftgripper.close()
pickgoal.pose.position.z = zn+0.1
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Move both arms to pos1
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Place object
stleft = PoseStamped()
stleft.header.frame_id = "base"
stleft.header.stamp = rospy.Time.now()
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.55
stleft.pose.position.z = 0.1
stleft.pose.orientation.x = 1.0
stleft.pose.orientation.y = 0.0
stleft.pose.orientation.z = 0.0
stleft.pose.orientation.w = 0.0
# Stack objects (large cubes) if size if greater than some threshold
if sz > 16.:
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.7
stleft.pose.position.z = -0.04+(k*0.05)
k += 1
gl.moveToPose(stleft, "left_gripper", plan_only=False)
leftgripper.open()
stleft.pose.position.z = 0.3
gl.moveToPose(stleft, "left_gripper", plan_only=False)
def dyn_wedge():
pub = rospy.Publisher("finished", String, queue_size=10)
# Initialize MoveIt scene
p = PlanningSceneInterface("base")
arms_group = MoveGroupInterface("both_arms", "base")
rightarm_group = MoveGroupInterface("right_arm", "base")
leftarm_group = MoveGroupInterface("left_arm", "base")
# Create right arm instance
right_arm = baxter_interface.limb.Limb('right')
# Create right gripper instance
right_gripper = baxter_interface.Gripper('right')
right_gripper.calibrate()
right_gripper.open()
right_gripper.close()
offset_zero_point=0.903
table_size_x = 0.714655654394
table_size_y = 1.05043717328
table_size_z = 0.729766045265
center_x = 0.457327827197
center_y = 0.145765166941
center_z = -0.538116977368
table_distance_from_gripper = -offset_zero_point+table_size_z+0.0275/2
j=0
k=0
# Initialize object list
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
p.clear()
p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
p.waitForSync()
# Move both arms to start state.
# Initial pose
rpos = PoseStamped()
rpos.header.frame_id = "base"
rpos.header.stamp = rospy.Time.now()
rpos.pose.position.x = 0.555
rpos.pose.position.y = 0.0
rpos.pose.position.z = 0.206 #0.18
rpos.pose.orientation.x = 1.0
rpos.pose.orientation.y = 0.0
rpos.pose.orientation.z = 0.0
rpos.pose.orientation.w = 0.0
lpos = PoseStamped()
lpos.header.frame_id = "base"
lpos.header.stamp = rospy.Time.now()
lpos.pose.position.x = 0.555
lpos.pose.position.y = 0.65
lpos.pose.position.z = 0.206#0.35
lpos.pose.orientation.x = 1.0
lpos.pose.orientation.y = 0.0
lpos.pose.orientation.z = 0.0
lpos.pose.orientation.w = 0.0
rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False)
# Get the middle point between the two centroids
locs = rospy.wait_for_message("clasificacion", PoseArray)
if(len(locs.poses)!=0):
punto_medio1 = PoseStamped()
punto_medio1.header.frame_id = "base"
punto_medio1.header.stamp = rospy.Time.now()
punto_medio1.pose.position.x = locs.poses[0].position.x
punto_medio1.pose.position.y = locs.poses[0].position.y
punto_medio1.pose.position.z = -0.08
punto_medio1.pose.orientation.x = locs.poses[0].orientation.x
punto_medio1.pose.orientation.y = locs.poses[0].orientation.y
punto_medio1.pose.orientation.z = locs.poses[0].orientation.z
punto_medio1.pose.orientation.w = locs.poses[0].orientation.w
else:
sys.exit("No se encuentran los puntos")
# Get the middle point again after a few seconds
tiempo = 3
time.sleep(tiempo)
locs = rospy.wait_for_message("clasificacion", PoseArray)
if(len(locs.poses)!=0):
punto_medio2 = PoseStamped()
punto_medio2.header.frame_id = "base"
punto_medio2.header.stamp = rospy.Time.now()
punto_medio2.pose.position.x = locs.poses[0].position.x
punto_medio2.pose.position.y = locs.poses[0].position.y
punto_medio2.pose.position.z = -0.08
punto_medio2.pose.orientation.x = locs.poses[0].orientation.x
punto_medio2.pose.orientation.y = locs.poses[0].orientation.y
punto_medio2.pose.orientation.z = locs.poses[0].orientation.z
punto_medio2.pose.orientation.w = locs.poses[0].orientation.w
else:
sys.exit("No se encuentran los puntos")
# Calculate speed of objects
vel = (punto_medio2.pose.position.y - punto_medio1.pose.position.y) / tiempo
# Predict position within a certain time
nuevo_tiempo = 2.1
posicion = nuevo_tiempo * vel * 2
if(posicion>=0):
nueva_y = punto_medio2.pose.position.y - posicion
else:
nueva_y = punto_medio2.pose.position.y + posicion
orientacion = (-1) * 1/punto_medio2.pose.orientation.x
new_or = 0.26
if orientacion > 0:
new_or = -0.26
# If there is time enough to sweep the objects
if vel > -0.08:
start = time.time()
punto_medio2.pose.position.y = nueva_y - nueva_y/2
punto_medio2.pose.position.x = punto_medio1.pose.position.x - 0.03
punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion - new_or)
rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
end = time.time()
tiempo = end - start
while(nuevo_tiempo - tiempo >= 1):
end = time.time()
nuevo_tiempo = end - start
else:
punto_medio2.pose.position.y = nueva_y
print(punto_medio2.pose.position.y)
punto_medio2.pose.position.x = punto_medio1.pose.position.x
punto_medio2.pose.position.y += 0.05
punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion)
rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
obj = punto_medio2.pose.position.y + 0.16
neg = 1
while punto_medio2.pose.position.y < obj:
punto_medio2.pose.position.y += 0.03
# Shake arm
punto_medio2.pose.position.x += neg * 0.09
rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
neg = (-1)*neg
time.sleep(2)
rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
freemem = "free"
pub.publish(freemem)
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
# Creat planning scene and move group to interact with robot
from moveit_msgs.msg import MoveItErrorCodes
from moveit_python import MoveGroupInterface, PlanningSceneInterface
# rospy.init_node("move_torso_up")
# Create move group interface for a fetch robot
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# Add table + object --> Hard coded, ! Have to lift arm beforehand, else stuck
planning_scene.removeCollisionObject("table")
planning_scene.addCube("table", 2, 1.1, 0.0, 0.82+0.1-1)
# Execute interpolated poses
for pose in approach_poses:
# Finish building the Pose_stamped message
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
obj_cts = 0
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
print " find goal"
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
print " find client"
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
print "the object %s should be removed" %(name)
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
print "the object %s should be removed" %(name)
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
print "-----------------------"
print obj.object.primitive_poses[0]
print " x: %d" %(obj.object.primitive_poses[0].position.x)
# if obj.object.primitive_poses[0].position.y > 0.0
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
use_service = False)
# def addSolidPrimitive (self, name, solid, pose, use_service=True)
print "1, %d" %(idx)
if obj.object.primitive_poses[0].position.x < 1.25:
objects.append([obj, obj.object.primitive_poses[0].position.z])
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
print "2,"
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
use_service = True
)
self.scene.waitForSync()
# store for grasping
#self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
# store graspable objects by Z
objects.sort(key=lambda object: object[1])
objects.reverse()
self.objects = [object[0] for object in objects]
rospy.loginfo("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
rospy.loginfo("number of objects...:::::::" + str(len(objects)))
#for object in objects:
# print(object[0].object.name, object[1])
#exit(-1)
def getGraspableObject(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
rospy.loginfo("must more than one object")
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25:
continue
# has to located in +y
if obj.object.primitive_poses[0].position.y < 0.0:
print "has to located in +y"
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
print "z has to larger than 0.5 "
continue
rospy.loginfo("object pose:")
print obj.object.primitive_poses[0], obj.object.primitives[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m-1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene=self.scene)
return success
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import rospy
from moveit_python import PlanningSceneInterface
if __name__ == "__main__":
rospy.init_node("list_objects")
scene = PlanningSceneInterface("base_link")
for name in scene.getKnownCollisionObjects():
print(name)
for name in scene.getKnownAttachedObjects():
print(name)
if len(scene.getKnownCollisionObjects() + scene.getKnownAttachedObjects()) == 0:
print("No objects in planning scene.")
class World:
def __init__(self, tower_array, disk_array, config, debug=0):
print "[INFO] Initialise World"
self.DEBUG = debug
# initialise arrays
self.tower_array = tower_array
self.disk_array = disk_array
# configs
self.max_gripper = config.get_max_gripper()
self.disk_height = config.disk_height
self.tower_positions = config.tower_positions
self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
"""Calls a method to display the collision objects.
"""
def create_planning_scene(self):
print "[INFO] Create_planning_scene"
self.display_towers_and_disks()
"""This method collects all needed information and
publish them to other methods.
"""
def display_towers_and_disks(self):
print "[INFO] Display towers and disks"
for tower in self.tower_array:
# call method to publish new tower
self.publish_scene(tower.get_position(), tower)
# set color by name
self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0)
# publish color
self.planning_scene_interface.sendColors()
for disk in self.disk_array:
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1],
disk.get_color()[2], disk.get_color()[3])
self.planning_scene_interface.sendColors()
# wait for sync after publishing collision objects
self.planning_scene_interface.waitForSync(5.0)
rospy.sleep(5)
"""This method creates a box or a cylinder with methods of
the planning scene interface.
:param position: the position of the new collision object
:param tower: the tower object
:param disk: the disk object
"""
def publish_scene(self, position, tower=None, disk=None):
if tower is not None:
self.planning_scene_interface.addBox(tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0,
(self.tower_positions[tower.get_id() - 1][2] * 2), position[0],
position[1], position[2])
else:
self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2,
position[0], position[1], position[2])
"""This method cleans the planning scene.
:param close: with this flag a new planning scene objects will be removed
in sync otherwise the object will be deleted without syncing the scene
"""
def clean_up(self, close=False):
if close:
# get the actual list after game
self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
for name in self.planning_scene_interface.getKnownCollisionObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing object %s" % name)
self.planning_scene_interface.removeCollisionObject(name, False)
for name in self.planning_scene_interface.getKnownAttachedObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing attached object %s" % name)
self.planning_scene_interface.removeAttachedObject(name, False)
if close:
self.planning_scene_interface.waitForSync(5.0)
pass
"""This method corrects the position of the moved disk.
:param tower: parent tower of the disk
"""
def refresh_disk_pose(self, tower):
print "[INFO] Refresh disk pose"
disk = tower.get_last()
if self.DEBUG is 1:
print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\
"tower pose", tower.get_position()
# remove the disk from planning scene
self.planning_scene_interface.removeCollisionObject(disk.get_id(), False)
# publish collision object and set old color
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1],
disk.get_color()[2], disk.get_color()[3])
self.planning_scene_interface.sendColors()
def setBoundaries():
'''
This is a fix for the FETCH colliding with itself
Define ground plane
This creates objects in the planning scene that mimic the ground
If these were not in place gripper could hit the ground
'''
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True)
self.move_group = MoveGroupInterface("left_arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server(rospy.Duration(15.0))
self.cubes = []
self.tf_listener = TransformListener()
# self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction)
# self.gripper_client.wait_for_server()
# rospy.loginfo("...connected.")
rospy.sleep(2.0)
def updateScene(self, remove_collision = False):
if remove_collision:
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
return
# find objects
self.cubes = []
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(15.0))
find_result = self.find_client.get_result()
# rospy.loginfo(find_result)
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
print(find_result.objects)
# print(find_result.support_surfaces)
# for obj in find_result.objects:
# idx += 1
# print(idx)
# obj.object.name = "object%d" % idx
# self.scene.addSolidPrimitive(obj.object.name,
# obj.object.primitives[0],
# obj.object.primitive_poses[0],
# wait = False)
# self.cubes.append(obj.object.primitive_poses[0])
#
# for obj in find_result.support_surfaces:
# # extend surface to floor, and make wider since we have narrow field of view
# height = obj.primitive_poses[0].position.z
# obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
# 1.5, # wider
# obj.primitives[0].dimensions[2] + height]
# obj.primitive_poses[0].position.z += -height / 2.0
#
# # add to scene
# self.scene.addSolidPrimitive(obj.name,
# obj.primitives[0],
# obj.primitive_poses[0],
# wait = False)
#
# self.scene.waitForSync()
#
# # store for grasping
# self.objects = find_result.objects
# self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def get_sequence(self, seq_list):
start = [self.objects[seq_list[0]].object, self.objects[seq_list[0]].grasps] or [None, None]
target = [self.objects[seq_list[1]].object, self.objects[seq_list[1]].grasps] or [None, None]
res = {'start': start, 'target': target}
return res
def get_transform_pose(self, pose):
point = PoseStamped()
point.header.frame_id = 'base_link'
point.header.stamp = rospy.Time()
point.pose = pose
return self.tf_listener.transformPose('/map', point).pose.position
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
retries = 5,
support_name = block.support_surface,
scene = self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene = self.scene,
retries = 5)
# print(success)
return success
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
# pose =[ 1.58, -0.056, -0.01, -1.31, -0.178, -0.13, 0.16]
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 0.0
gripper_goal.command.position = 0.09
self.gripper_client.send_goal(gripper_goal)
self.gripper_client.wait_for_result(rospy.Duration(5))
start = rospy.Time.now()
while rospy.Time.now() - start <= rospy.Duration(10.0): # rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
return
def gripper_opening(self, opening = 0.09):
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 0.0
gripper_goal.command.position = opening
self.gripper_client.send_goal(gripper_goal)
self.gripper_client.wait_for_result(rospy.Duration(5))
def move():
rospy.init_node("move")
move_group = MoveGroupInterface("arm_with_torso", "base_link")
#planning scene setup
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
#move head camera
client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
rospy.loginfo("Waiting for head_controller...")
client.wait_for_server()
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = "base_link"
goal.target.point.x = 1.2
goal.target.point.y = 0.0
goal.target.point.z = 0.0
goal.min_duration = rospy.Duration(1.0)
client.send_goal(goal)
client.wait_for_result()
#arm setup
move_group = MoveGroupInterface("arm", "base_link")
#set arm initial position
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def __init__(self):
self.moving_mode = False
self.waldo_mode = False
self.prev_joy_buttons = 0
# See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
self.joint_names = [
#'torso_lift_joint',
#'bellows_joint',
#'head_pan_joint',
#'head_tilt_joint',
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
'l_gripper_finger_joint',
#'r_gripper_finger_joint',
]
self.joint_name_map = dict([
(name, index) for index, name in enumerate(self.joint_names)
])
self.cur_joint_pos = np.zeros([len(self.joint_names)])
self.cur_joint_vel = np.zeros([len(self.joint_names)])
self.cur_joint_effort = np.zeros([len(self.joint_names)])
self.cur_base_scan = np.zeros([662], dtype=np.float32)
self.cur_head_camera_depth_image = np.zeros([480, 640],
dtype=np.float32)
self.cur_head_camera_rgb_image = np.zeros([480, 640, 3],
dtype=np.uint8)
self.timeseq = None
self.timeseq_mutex = threading.Lock()
self.image_queue = Queue.Queue()
self.image_compress_thread = threading.Thread(
target=self.image_compress_main)
self.image_compress_thread.daemon = True
self.image_compress_thread.start()
logger.info('Subscribing...')
self.subs = []
if 1:
self.subs.append(
rospy.Subscriber('/joint_states', JointState,
self.joint_states_cb))
if 0:
self.subs.append(
rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
if 1:
self.subs.append(
rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image),
self.head_camera_depth_image_cb))
if 1:
self.subs.append(
rospy.Subscriber('/head_camera/rgb/image_raw',
numpy_msg(Image),
self.head_camera_rgb_image_raw_cb))
if 1:
self.subs.append(
rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
logger.info('Subscribed')
self.arm_effort_pub = rospy.Publisher(
'/arm_controller/weightless_torque/command',
JointTrajectory,
queue_size=2)
#self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
self.gripper_client = actionlib.SimpleActionClient(
'gripper_controller/gripper_action', GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher(
'/arm_controller/cartesian_twist/command', Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient(
'head_controller/point_head', PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm",
"base_link",
plan_only=True)
self.arm_trajectory_client = actionlib.SimpleActionClient(
"arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.arm_trajectory_client.wait_for_server()
if 0:
logger.info('Creating MoveGroupInterface...')
self.move_group = MoveGroupInterface('arm_with_torso',
'base_link',
plan_only=True)
logger.info('Created MoveGroupInterface')
if 0:
logger.info('Creating PlanningSceneInterface...')
self.planning_scene = PlanningSceneInterface('base_link')
self.planning_scene.removeCollisionObject('my_front_ground')
self.planning_scene.removeCollisionObject('my_back_ground')
self.planning_scene.removeCollisionObject('my_right_ground')
self.planning_scene.removeCollisionObject('my_left_ground')
self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0,
-1.0)
self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0,
-1.0)
self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2,
-1.0)
self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2,
-1.0)
logger.warn('FetchRobotGymEnv ROS node running')
self.head_point_client.wait_for_server()
logger.warn('FetchRobotGymEnv ROS node connected')
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
def main():
rospy.init_node('a5_obstacles')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('floor')
planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2)
planning_scene.addBox('table', 0.5, 1, 0.82, 0.8, 0, 0.72 / 2)
planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2)
rospy.sleep(2)
rospy.spin()
def __init__(self):
self.moving_mode = False
self.plan_only = False
self.prev_joy_buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
self.joy_subscriber = rospy.Subscriber("/fetch_joy", Joy,
self.joy_callback)
self.joints_subscriber = rospy.Subscriber("/joint_states", JointState,
self.joints_states_callback)
self.rgbimage_subscriber = rospy.Subscriber("/head_camera/depth/image",
Image,
self.depthimage_callback)
self.depthimage_subscriber = rospy.Subscriber(
"/head_camera/depth/image", Image, self.rgbimage_callback)
self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_reset_pos",
Empty,
self.arm_reset_callback)
self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_start_pos",
Empty,
self.arm_start_callback)
self.arm_effort_pub = rospy.Publisher(
"/arm_controller/weightless_torque/command",
JointTrajectory,
queue_size=2)
self.gripper_client = actionlib.SimpleActionClient(
"gripper_controller/gripper_action", GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher(
"/arm_controller/cartesian_twist/command", Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient(
"head_controller/point_head", PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm",
"base_link",
plan_only=self.plan_only)
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# Note: roslaunch fetch_moveit_config move_group.launch must be running
# Safety!: Do NOT run this script near people or objects.
# Safety!: There is NO perception.
# The ONLY objects the collision detection software is aware
# of are itself & the floor.
if __name__ == '__main__':
rospy.init_node("fit_moveit")
# Create move group interface for a fetch robot
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
# planning_scene.removeCollisionObject("my_front_ground")
# planning_scene.removeCollisionObject("my_back_ground")
# planning_scene.removeCollisionObject("my_right_ground")
# planning_scene.removeCollisionObject("my_left_ground")
# planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
# planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
# planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
# planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# This is the wrist link not the gripper itself
gripper_frame = 'wrist_roll_link'
# Position and rotation of two "wave end poses"
pose = Pose(Point(0.17, 1.545, 1.822),
Quaternion(-0.374, -0.701, 0.173, 0.635))
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
rospy.loginfo("Found %d objects" % len(find_result.objects))
# remove previous objects
#for name in self.scene.getKnownCollisionObjects():
# self.scene.removeCollisionObject(name, False)
#for name in self.scene.getKnownAttachedObjects():
# self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "obj%d"%idx
self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], use_service=False)
if obj.object.primitive_poses[0].position.x < 1.3: #< 0.85:
objects.append([obj, obj.object.primitive_poses[0].position.y])
#for obj in find_result.support_surfaces:
# # extend surface to floor, and make wider since we have narrow field of view
# height = obj.primitive_poses[0].position.z
# obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider
# obj.primitives[0].dimensions[2] + height]
# obj.primitive_poses[0].position.z += -height/2.0
# # add to scene
# self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0])
self.scene.waitForSync()
# store for grasping
self.surfaces = find_result.support_surfaces
# store graspable objects by y
objects.sort(key=lambda object: object[1])
#objects.reverse()
self.objects = [object[0] for object in objects]
def getGraspableObject(self, goal_obj_x, goal_obj_y):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.4:
continue
if obj.object.primitive_poses[0].position.x < 0.3:
continue
# get goal object
if ( abs(obj.object.primitive_poses[0].position.x - goal_obj_x) > 0.03):
continue
if ( abs(obj.object.primitive_poses[0].position.y - goal_obj_y) > 0.03):
continue
print("**************************************Object Name: ", obj.object.name)
print("Object Pose: ", obj.object.primitive_poses[0], obj.object.primitives[0])
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, scene=self.scene)
self.pick_result = pick_result
#hold the object
#touch_links = ['l_gripper_finger_link', 'r_gripper_finger_link']
#self.hand_group.attach_object('apple', 'base_link', touch_links=touch_links)
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m-1):
#l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene)
return success
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
#pose = [-1.60, -1.10, -1.20, -1.50, 0.0, -1.51, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class Grasping(object):
def __init__(self):
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.scene = PlanningSceneInterface("base_link")
self.move_group = MoveGroupInterface("arm", "base_link")
find_objects = "basic_grasping_perception/find_objects"
self.find_client = actionlib.SimpleActionClient(
find_objects, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def pickup(self, block, grasps):
rospy.sleep(1)
success, pick_result = self.pickplace.pick_with_retry(
block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
#swaps two blocks positions.
def swapBlockPos(self, block1Pos, block2Pos):
#intermediate point for movement
self.armIntermediatePose()
#intermediate positon on the table, used for short term storage of the first block manipulated in the sorting.
posIntermediate = np.array([0.725, 0])
self.armIntermediatePose()
# Get block to pick
while not rospy.is_shutdown():
rospy.loginfo("Picking object...")
self.updateScene()
cube, grasps = self.getGraspableObject(block1Pos)
if cube == None:
rospy.logwarn("Perception failed.")
continue
# Pick the block
if self.pickup(cube, grasps):
break
rospy.logwarn("Grasping failed.")
#self.tuck()
#Place the block
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = cube.primitive_poses[0]
pose.pose.position.z += 0.05
pose.header.frame_id = cube.header.frame_id
if self.place(cube, pose, posIntermediate):
break
rospy.logwarn("Placing failed.")
#place block 2 in block 1's
self.armIntermediatePose()
# Get block to pick
while not rospy.is_shutdown():
rospy.loginfo("Picking object...")
self.updateScene()
cube, grasps = self.getGraspableObject(block2Pos)
if cube == None:
rospy.logwarn("Perception failed.")
continue
# Pick the block
if self.pickup(cube, grasps):
break
rospy.logwarn("Grasping failed.")
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = cube.primitive_poses[0]
pose.pose.position.z += 0.05
pose.header.frame_id = cube.header.frame_id
if self.place(cube, pose, block1Pos):
break
rospy.logwarn("Placing failed.")
#place block1 in block 2's spot
self.armIntermediatePose()
while not rospy.is_shutdown():
rospy.loginfo("Picking object...")
self.updateScene()
cube, grasps = self.getGraspableObject(posIntermediate)
if cube == None:
rospy.logwarn("Perception failed.")
continue
# Pick the block
if self.pickup(cube, grasps):
break
rospy.logwarn("Grasping failed.")
#self.tuck()
self.armIntermediatePose()
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = cube.primitive_poses[0]
pose.pose.position.z += 0.05
pose.header.frame_id = cube.header.frame_id
if self.place(cube, pose, block2Pos):
break
rospy.logwarn("Placing failed.")
return
def place(self, block, pose_stamped, placePos):
rospy.sleep(1)
#creates a list of place positons for the block, with a specified x, y, and z.
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = "base_link"
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
#this x and y value are input as placePos through the function call.
l.place_pose.pose.position.x = placePos[0]
l.place_pose.pose.position.y = placePos[1]
places.append(copy.deepcopy(l))
#success = self.pickplace.place_with_retry(block.name, places, scene = self.scene)
#return success
## create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, scene=self.scene)
return success
def updateScene(self):
rospy.sleep(1)
# find objectsw
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
use_service=False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
use_service=True)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def getGraspableObject(self, pos):
graspable = None
for obj in self.objects:
if len(obj.grasps) < 1:
continue
if (obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[1] < 0.05 or \
obj.object.primitives[0].dimensions[1] > 0.07 or \
obj.object.primitives[0].dimensions[2] < 0.05 or \
obj.object.primitives[0].dimensions[2] > 0.07):
continue
if (obj.object.primitive_poses[0].position.z < 0.3) or \
(obj.object.primitive_poses[0].position.y > pos[1]+0.05) or \
(obj.object.primitive_poses[0].position.y < pos[1]-0.05) or \
(obj.object.primitive_poses[0].position.x > pos[0]+0.05) or \
(obj.object.primitive_poses[0].position.x < pos[0]-0.05):
continue
return obj.object, obj.grasps
return None, None
def armToXYZ(self, x, y, z):
rospy.sleep(1)
#new pose_stamped of the end effector that moves the arm out of the way of the vision for planning.
intermediatePose = PoseStamped()
intermediatePose.header.frame_id = 'base_link'
#position
intermediatePose.pose.position.x = x
intermediatePose.pose.position.y = y
intermediatePose.pose.position.z = z
#quaternion for the end position
intermediatePose.pose.orientation.w = 1
while not rospy.is_shutdown():
result = self.move_group.moveToPose(intermediatePose,
"wrist_roll_link")
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def armIntermediatePose(self):
self.armToXYZ(0.1, -0.7, 0.9)
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
parser = argparse.ArgumentParser(
description="Remove objects from the MoveIt planning scene.")
parser.add_argument("name",
nargs="?",
help="Name of object to remove")
parser.add_argument("--all",
help="Remove all objects.",
action="store_true")
parser.add_argument("--attached",
help="Remove an attached object.",
action="store_true")
args = parser.parse_args()
if args.all:
rospy.init_node("remove_objects")
scene = PlanningSceneInterface("base_link")
for name in scene.getKnownCollisionObjects():
print("Removing %s" % name)
scene.removeCollisionObject(name, use_service=False)
scene.waitForSync()
elif args.name:
rospy.init_node("remove_objects")
scene = PlanningSceneInterface("base_link")
print("Removing %s" % args.name)
if args.attached:
scene.removeAttachedObject(args.name)
else:
scene.removeCollisionObject(args.name)
else:
parser.print_help()
def __init__(self):
self._scene = PlanningSceneInterface('base_link')
self._pickplace = PickPlaceInterface('xarm5', 'gripper', verbose=True)
self._move_group = MoveGroupInterface('xarm5', 'base_link')
class GraspingClient(object):
def __init__(self, sim=False):
# Define planning groups
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
planner_id = "RRTConnectkConfigDefault"
self.move_group.setPlannerId(planner_id)
self.rmove_group.setPlannerId(planner_id)
self.objects_heights = [0.122, 0.240]
self.objects_heights_tolerances = [0.02, 0.03]
# Define joints and positions for 6 and 7 DOF
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
#self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
self.constrained_stow = [
-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
0.0, -1.0, 0.42, 0, 0
]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
# Define the MoveIt pickplace pipeline
self.pickplace = None
self.pickplace = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace.planner_id = planner_id
self.pick_result = None
self.place_result = None
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
find_grasp_planning_topic = "grasp_planner/plan"
rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
self.find_grasp_planning_client = actionlib.SimpleActionClient(
find_grasp_planning_topic, GraspPlanningAction)
self.find_grasp_planning_client.wait_for_server()
rospy.loginfo("...connected")
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
def __del__(self):
self.scene.clear()
def add_objects_to_keep(self, obj):
self._objs_to_keep.append(obj)
def clearScene(self):
self.scene.clear()
def calculateGraspForObject(self, object_to_grasp, gripper):
goal = GraspPlanningGoal()
goal.object = object_to_grasp
goal.gripper = gripper
self.find_grasp_planning_client.send_goal(goal)
self.find_grasp_planning_client.wait_for_result(rospy.Duration(5.0))
return self.find_grasp_planning_client.get_result(
).grasps #moveit_msgs/Grasp[]
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
retries=1,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, retries=1, scene=self.scene)
self.place_result = place_result
return success
def goto_tuck(self):
# remove previous objects
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.tucked, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.constrained_stow, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def left_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "left_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.lmove_group.moveToJointPosition(
self._left_arm_joints,
self.larm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def right_arm_constrained_stow(self):
# c1 = Constraints()
# c1.orientation_constraints.append(OrientationConstraint())
# c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
# c1.orientation_constraints[0].header.frame_id = "base_link"
# c1.orientation_constraints[0].link_name = "right_ee_link"
# c1.orientation_constraints[0].orientation.w=1.0
# c1.orientation_constraints[0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
# c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
# c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
# c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.rmove_group.moveToJointPosition(
self._right_arm_joints,
self.rarm_const_stow,
0.05,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def open_gripper(self):
self._lgripper.command(self._gripper_open, block=True)
def close_gripper(self):
self._lgripper.command(self._gripper_closed, block=True)
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.model_pose = [0.0,0.0,0.0]
self.set_forbidden()
self.set_init_pose()
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def set_init_pose(sefl):
#set init pose
move_group = MoveGroupInterface("manipulator","base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint",
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
move_group.get_move_action().cancel_all_goals()
def callback(self,data):
#print data.markers[0].pose.position
x = data.markers[0].pose.position.x
y = data.markers[0].pose.position.y
z = data.markers[0].pose.position.z
self.model_pose = [x,y,z]
def start_subscriber(self):
rospy.Subscriber("/visualization_marker_array", MarkerArray, self.callback)
def publish_tf(self):
br = tf.TransformBroadcaster()
br.sendTransform((self.model_pose[0], self.model_pose[1], self.model_pose[2]), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "object", "depth_camera_frame")
def target(self):
rate = rospy.Rate(1)
rospy.sleep(1)
ls = tf.TransformListener()
while not rospy.is_shutdown():
self.publish_tf()
try:
(trans,rot) = ls.lookupTransform('/base_link', '/object', rospy.Time(0))
print "tf base_link to test",trans
group = moveit_commander.MoveGroupCommander("manipulator")
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = pose.orientation.x
pose_target.orientation.y = pose.orientation.y
pose_target.orientation.z = pose.orientation.z
pose_target.orientation.w = pose.orientation.w
pose_target.position.x = trans[0]
pose_target.position.y = trans[1]
pose_target.position.z = trans[2]+0.2
group.set_pose_target(pose_target)
group.go()
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "e"
rospy.sleep(1)
self.set_init_pose()
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
rospy.loginfo("get new grasps")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TODO: ADD BOX
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
# added + .1
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def picknplace():
# Define initial parameters
p = PlanningSceneInterface("base")
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# Clear planning scene
p.clear()
# Add table as attached object
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# Move both arms to start state
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Loop to continue pick and place until all objects are cleared from table
j=0
k=0
while locs_x:
p.clear() # CLear planning scene of all collision objects
# Attach table as attached collision object to base of baxter
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
xn = locs_x[0]
yn = locs_y[0]
zn = -0.06
thn = orien[0]
sz = size[0]
if thn > pi/4:
thn = -1*(thn%(pi/4))
# Add collision objects into planning scene
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
for i in range(1,len(locs_x)):
p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
p.waitForSync()
# Move both arms to pos2 (Right arm away and left arm on table)
g.moveToJointPosition(jts_both, pos2, plan_only=False)
# Move left arm to pick object and pick object
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn+0.1
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles
pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
pickgoal.pose.position.z = zn
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
leftgripper.close()
pickgoal.pose.position.z = zn+0.1
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Move both arms to pos1
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Place object
stleft = PoseStamped()
stleft.header.frame_id = "base"
stleft.header.stamp = rospy.Time.now()
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.55
stleft.pose.position.z = 0.1
stleft.pose.orientation.x = 1.0
stleft.pose.orientation.y = 0.0
stleft.pose.orientation.z = 0.0
stleft.pose.orientation.w = 0.0
# Stack objects (large cubes) if size if greater than some threshold
if sz > 16.:
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.7
stleft.pose.position.z = -0.04+(k*0.05)
k += 1
gl.moveToPose(stleft, "left_gripper", plan_only=False)
leftgripper.open()
stleft.pose.position.z = 0.3
gl.moveToPose(stleft, "left_gripper", plan_only=False)
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True)
self.robot = moveit_commander.RobotCommander()
# self.model_pose = [0.0,0.2,0.2 , 0,0,0,1]
self.model_pose = [0.0, 0.3, 0.1, 0, 0, 0, 1]
self.obstacle_pose = [0.0, 0.0, 0.0]
self.set_forbidden()
self.set_init_pose()
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_ground")
self.planning_scene.addCube("my_ground", 2, 0, 0, -1.04)
self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5)
self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5)
#self.planning_scene.addCube("demo_cube", 0.06,0,0.3,0)
print dir(self.planning_scene)
import inspect
print "addBox's variable is ", inspect.getargspec(
self.planning_scene.addBox)
print "attachBox's variable is ", inspect.getargspec(
self.planning_scene.attachBox)
print "addCube's variable is ", inspect.getargspec(
self.planning_scene.addCube)
print "setColor's variable is ", inspect.getargspec(
self.planning_scene.setColor)
def set_init_pose(sefl):
#set init pose
move_group = MoveGroupInterface("manipulator", "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
pose = [-1.26, -0.64, -2.44, -0.66, 1.56, 0.007]
move_group.moveToJointPosition(joint_names, pose, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
move_group.get_move_action().cancel_all_goals()
def callback(self, data):
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
def start_subscriber(self):
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
def reaching_pose(self):
print self.model_pose
group = moveit_commander.MoveGroupCommander("manipulator")
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = self.model_pose[3]
pose_target.orientation.y = self.model_pose[4]
pose_target.orientation.z = self.model_pose[5]
pose_target.orientation.w = self.model_pose[6]
pose_target.position.x = self.model_pose[0]
pose_target.position.y = self.model_pose[1]
pose_target.position.z = self.model_pose[2]
group.set_pose_target(pose_target)
group.go()
#raw_input("Enter -->")
def gripper_close(self):
print("Gripper_Close")
gripper_pose = Char()
gripper_pose = 255
pub.publish(gripper_pose)
#raw_input("Enter -->")
def gripper_open(self):
print("Gripper_Open")
gripper_pose = Char()
gripper_pose = 0
pub.publish(gripper_pose)
#raw_input("Enter -->")
def move_45(self):
rate = rospy.Rate(1)
rospy.sleep(1)
a = 0.065
b = 0.040
c = 0.012
pattern_45 = [
[-0.10, 0.31, 0.015, 0, 0, 0, 1], #B
'c',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
#[0.0 ,0.31 ,0.15 , 0,0,0,1], #A90
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
[0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795],
'o',
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
#[0.0 ,0.31 ,0.1 , 0,0,0,1], #A90
[-0.10, 0.401, 0.041, 0, 0, 0, 1],
'c',
[-0.10, 0.401, 0.100, 0, 0, 0, 1],
[0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795],
[0, 0.5 - b, 0.037 + b, 0.3826834, 0, 0, 0.9238795],
[0, 0.5 - c, 0.037 + c, 0.3826834, 0, 0, 0.9238795],
'o',
'c',
[0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795],
[-0.10, 0.401, 0.100, 0, 0, 0, 1],
[-0.10, 0.401, 0.050, 0, 0, 0, 1],
'o',
[-0.10, 0.401, 0.100, 0, 0, 0, 1],
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
[0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795],
'c',
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
[-0.10, 0.31, 0.015, 0, 0, 0, 1], #B
'o',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
]
while not rospy.is_shutdown():
for pose in pattern_45:
if pose == 'c':
self.gripper_close()
rospy.sleep(1)
elif pose == 'o':
self.gripper_open()
rospy.sleep(1)
else:
self.model_pose = pose
self.reaching_pose()
if rospy.is_shutdown():
return 0
def move_90(self):
rate = rospy.Rate(1)
rospy.sleep(1)
a = 0.065
b = 0.040
c = 0.012
pattern_45 = [
[-0.10, 0.31, 0.015, 0, 0, 0, 1], #B
'c',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
[0.0, 0.300, 0.1, 0, 0, 0, 1], #A90
[0.0, 0.315, 0.0358, 0, 0, 0, 1], #A90
'o',
[0.0, 0.315, 0.1, 0, 0, 0, 1], #A90
[-0.10, 0.40, 0.041, 0, 0, 0, 1], #negi
'c',
[-0.10, 0.40, 0.1, 0, 0, 0, 1],
[-0.001, 0.316, 0.1, 0, 0, 0, 1], #A90
[-0.001, 0.316, 0.053, 0, 0, 0, 1], #A90
'o',
'c',
[0.0, 0.314, 0.1, 0, 0, 0, 1], #A90
[-0.10, 0.40, 0.1, 0, 0, 0, 1],
[-0.10, 0.40, 0.05, 0, 0, 0, 1], #negi
'o',
#[-0.10,0.40 ,0.1 , 0,0,0,1],
[0.0, 0.300, 0.1, 0, 0, 0, 1], #A90
[0.0, 0.315, 0.036, 0, 0, 0, 1], #A90
'c',
[0.0, 0.300, 0.1, 0, 0, 0, 1], #A90
[-0.10, 0.320, 0.1, 0, 0, 0, 1], #B
[-0.10, 0.31, 0.0153, 0, 0, 0, 1], #B
'o',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
]
while not rospy.is_shutdown():
for pose in pattern_45:
if pose == 'c':
self.gripper_close()
rospy.sleep(1)
elif pose == 'o':
self.gripper_open()
rospy.sleep(1)
else:
self.model_pose = pose
self.reaching_pose()
if rospy.is_shutdown():
return 0
from moveit_msgs.msg import CollisionObject #, AttachedCollisionObject
from shape_msgs.msg import SolidPrimitive
from math import pi, sqrt, cos, sin
from copy import deepcopy
import numpy as np
import baxter_pykdl as kdl
from tf.transformations import *
#################################### INITIALISATION ####################################
# Init node
rospy.init_node('kinect_pnp', anonymous=True)
# Baxter Kinematics
rightSolver = kdl.baxter_kinematics('right')
leftSolver = kdl.baxter_kinematics('left')
# Initialize the planning scene interface.
p = PlanningSceneInterface("base")
# Connect the arms to the move group.
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
# Create baxter_interface limb instance.
rightarm = baxter_interface.limb.Limb('right')
leftarm = baxter_interface.limb.Limb('left')
# Create baxter_interface gripper instance.
rightgripper = baxter_interface.Gripper('right')
leftgripper = baxter_interface.Gripper('left')
# Constant
bskposR = PoseStamped()
bskposR.header.frame_id = "base"
bskposR.header.stamp = rospy.Time.now()
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True)
self.model_pose = [0.0, 0.0, 0.0]
self.set_init_pose()
#self.set_forbidden()
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.planning_scene.removeCollisionObject("demo_cube")
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface("arm_with_torso", "base_link")
#set arm initial position
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
#pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pose = [0.385, -1.0, -1.0, 0.0, 1.0, 0.0, 1.5, 0.0]
#pose = [0.385, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
result = move_group.moveToJointPosition(joints, pose, 0.02)
def target(self):
rate = rospy.Rate(1)
rospy.sleep(1)
ls = tf.TransformListener()
while not rospy.is_shutdown():
print self.model_pose
try:
(trans, rot) = ls.lookupTransform('/base_link', '/object',
rospy.Time(0))
print "tf base_link to test", trans
group = moveit_commander.MoveGroupCommander("arm_with_torso")
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = pose.orientation.x
pose_target.orientation.y = pose.orientation.y
pose_target.orientation.z = pose.orientation.z
pose_target.orientation.w = pose.orientation.w
pose_target.position.x = trans[0]
pose_target.position.y = trans[1]
pose_target.position.z = trans[2] + 0.3
group.set_pose_target(pose_target)
group.go()
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print "e"
rospy.sleep(1)
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(
block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the postrection
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, scene=self.scene)
return success
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class GripperInterfaceClient:
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def execute_trajectory(self, trajectory):
goal_position = trajectory.points[-1].positions
print "==========", goal_position
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position[0] + goal_position[1]
command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this
self.client.send_goal(command_goal)
self.client.wait_for_result()
def move_to(self, goal_position, max_effort):
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position
command_goal.command.max_effort = max_effort
self.client.send_goal(command_goal)
self.client.wait_for_result()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(
obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False
)
# Try mesh representation
# idx = -1
# for obj in find_result.objects:
# idx += 1
# obj.object.name = "object%d"%idx
# self.scene.addMesh(obj.object.name,
# obj.object.mesh_poses[0],
# obj.object.meshes[0],
# wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0] + 0.1,
2.5, # wider
obj.primitives[0].dimensions[2] + height,
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
# if obj.object.primitives[0].dimensions[0] < 0.03 or \
# obj.object.primitives[0].dimensions[0] > 0.04 or \
# obj.object.primitives[0].dimensions[0] < 0.07 or \
# obj.object.primitives[0].dimensions[0] > 0.09 or \
# obj.object.primitives[2].dimensions[2] < 0.19 or \
# obj.object.primitives[2].dimensions[2] > 0.20:
# continue
# has to be on table
# if obj.object.primitive_poses[0].position.z < 0.5:
# continue
return obj.object, obj.grasps
# nothing detected
return None, None
def plan_pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(
block.name,
grasps,
support_name=block.support_surface,
allow_gripper_support_collision=False,
planner_id="PRMkConfigDefault",
retries=2,
scene=self.scene,
)
# self.pick_result = pick_result
if success:
return pick_result.trajectory_stages
else:
rospy.loginfo("Planning Failed.")
return None
def __init__(self, sim=False):
# Define planning groups
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
planner_id = "RRTConnectkConfigDefault"
self.move_group.setPlannerId(planner_id)
self.rmove_group.setPlannerId(planner_id)
self.objects_heights = [0.122, 0.240]
self.objects_heights_tolerances = [0.02, 0.03]
# Define joints and positions for 6 and 7 DOF
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
#self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
self.constrained_stow = [
-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
0.0, -1.0, 0.42, 0, 0
]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
# Define the MoveIt pickplace pipeline
self.pickplace = None
self.pickplace = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace.planner_id = planner_id
self.pick_result = None
self.place_result = None
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
find_grasp_planning_topic = "grasp_planner/plan"
rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
self.find_grasp_planning_client = actionlib.SimpleActionClient(
find_grasp_planning_topic, GraspPlanningAction)
self.find_grasp_planning_client.wait_for_server()
rospy.loginfo("...connected")
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
rospy.loginfo("Add collision objects")
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.removeCollisionObject("box_1")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.scene.addBox("box_1", 0.44, 0.43, 0.68, 0.7, -0.4, .34)
self.scene.waitForSync()
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
