def main():
rospy.init_node('part1_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)
table_height = 0.767
table_width = 0.7
table_x = 0.95
planning_scene.addBox('table', table_width, 2, table_height, table_x, 0,
table_height / 2)
planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2)
microwave_height = 0.28
microwave_width = 0.48
# microwave_depth = 0.33
microwave_depth = 0.27
microwave_x = 0.97
microwave_z = 0.06
microwave_y = 0.18
planning_scene.addBox('microwave', microwave_depth, microwave_width,
microwave_height, microwave_x, microwave_y,
table_height + microwave_z + microwave_height / 2)
rospy.sleep(2)
rospy.spin()
def tuck():
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
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 = client.moveToJointPosition(joints,
pose,
0.0,
max_velocity_scaling_factor=0.5)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
move_thread.stop()
return
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 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 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.72, 1, 0, 0.72 / 2)
rospy.sleep(2)
def main():
rospy.init_node('a5_obstacles')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('top_shelf')
planning_scene.removeCollisionObject('bottom_shelf')
planning_scene.addBox('top_shelf', 0.32, 1, 0.4, 0.99, 0, 1.64)
planning_scene.addBox('bottom_shelf', 0.5, 1, 1.09, 0.9, 0, 0.545)
rospy.sleep(2)
class Arm:
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6)
def MoveToPose(self, X, Y, Z, Roll, Pitch, Yaw):
orientation = tf.transformations.quaternion_from_euler(
Roll, Pitch, Yaw)
self.poseStamped = PoseStamped()
self.poseStamped.header.frame_id = 'base_link'
self.poseStamped.header.stamp = rospy.Time.now()
self.poseStamped.pose.position.x = X
self.poseStamped.pose.position.y = Y
self.poseStamped.pose.position.z = Z
self.poseStamped.pose.orientation.x = orientation[0]
self.poseStamped.pose.orientation.y = orientation[1]
self.poseStamped.pose.orientation.z = orientation[2]
self.poseStamped.pose.orientation.w = orientation[3]
self.moveGroup.moveToPose(self.poseStamped,
'gripper_link',
0.005,
max_velocity_scaling_factor=0.2)
self.result = self.moveGroup.get_move_action().get_result()
def Tuck(self):
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]
self.moveGroup.moveToJointPosition(joints,
pose,
0.01,
max_velocity_scaling_factor=0.2)
while not rospy.is_shutdown():
self.result = self.moveGroup.get_move_action().get_result()
if self.result.error_code.val == MoveItErrorCodes.SUCCESS:
break
def AddDice(self, name, X, Y, Z):
self.planning_scene.addCube(name, 0.1, X, Y, Z)
def RemoveDice(self, name):
self.planning_scene.removeCollisionObject(name)
class GripperTeleopDown(GripperTeleop):
def __init__(self, arm, gripper, im_server):
super(GripperTeleopDown, self).__init__(arm, gripper, im_server)
# subscribes to the visualization marker for the table coming from perception
self._planning_scene = PlanningSceneInterface('base_link')
self._table_sub = rospy.Subscriber('/visualization_marker', Marker,
self.table_callback)
def table_callback(self, msg):
if msg.ns == 'table':
self._planning_scene.removeCollisionObject('table')
# TODO: use the msg.pose.orientation to conver the scale directly
# instead of manually swapping x and y
x = min(msg.scale.y, msg.scale.x)
y = max(msg.scale.y, msg.scale.x)
#self._planning_scene.addBox('table', x, y, msg.scale.z, msg.pose.position.x, msg.pose.position.y, msg.pose.position.z)
if msg.ns == 'tray handle':
#handle_im = create_handle_interactive_marker(create_pose_stamped(msg.pose).pose, msg.scale)
pose = create_pose_stamped(msg.pose).pose
pose.orientation = self._constraint_pose.orientation
pose.position.z += 0.3
handle_im = create_gripper_interactive_marker(
pose, 'handle', False, False)
self._im_server.insert(handle_im, feedback_cb=self.handle_feedback)
self._im_server.applyChanges()
def start(self):
mat = tft.identity_matrix()
mat[:, 0] = np.array([0, 0, -1, 0])
mat[:, 2] = np.array([1, 0, 0, 0])
o = tft.quaternion_from_matrix(mat)
self._constraint_pose = Pose(orientation=Quaternion(*o))
gripper_im = create_gripper_interactive_marker(self._constraint_pose,
pregrasp=False,
rotation_enabled=False)
self._im_server.insert(gripper_im, feedback_cb=self.handle_feedback)
self._im_server.applyChanges()
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'gripper_link'
oc.orientation = self._constraint_pose.orientation
oc.weight = 1.0
oc.absolute_z_axis_tolerance = 0.1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
self.contraint = oc
def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
else:
rospy.loginfo("moveit already started")
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")
keepout_pose = Pose()
keepout_pose.position.z = 0.375
keepout_pose.orientation.w = 1.0
ground_pose = Pose()
ground_pose.position.z = -0.03
ground_pose.orientation.w = 1.0
rospack = rospkg.RosPack()
mesh_dir = os.path.join(rospack.get_path('fetch_teleop'), 'mesh')
scene.addMesh('keepout', keepout_pose,
os.path.join(mesh_dir, 'keepout.stl'))
scene.addMesh('ground', ground_pose,
os.path.join(mesh_dir, 'ground.stl'))
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")
scene.removeCollisionObject("ground")
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 main():
rospy.init_node('part2_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)
table_height = 0.767
table_width = 0.7
table_x = 0.95
planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2)
planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2)
rospy.sleep(2)
rospy.spin()
def setup_scene():
scene = PlanningSceneInterface('base_link')
scene.removeCollisionObject('box')
scene.addCube('box', 0.25, -0.45, 0.1, 0.125)
pick_place = PickPlaceInterface('xarm5', 'gripper', verbose=True)
grasp = Grasp()
# fill in g
# setup object named object_name using PlanningSceneInterface
pick_place.pickup('box', [grasp], support_name='supporting_surface')
place_loc = PlaceLocation()
# fill in l
pick_place.place('box'[place_loc],
goal_is_eef=True,
support_name='supporting_surface')
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
def arm_move(self):
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_left_ground", 1, 1.5,
self.translation[2][0], self.translation[2][1],
self.translation[2][2])
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"
]
# Lists of joint angles in the same order as in joint_names
disco_poses = [[0, 2.5, -0.1, 3.0, 1.5, 3.0, 1.0, 3.0]]
for pose in disco_poses:
if rospy.is_shutdown():
break
# Plans the joints in joint_names to angles in pose
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Disco!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
class ArmController(object):
def __init__(self):
# Create move group interface for a fetch robot
self._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
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 SetPose(self, target_pose):
# Plans the joints in joint_names to angles in pose
self._move_group.moveToJointPosition(target_pose.keys(),
target_pose.values(),
wait=False)
# Since we passed in wait=False above we need to wait here
self._move_group.get_move_action().wait_for_result()
result = self._move_group.get_move_action().get_result()
return_result = False
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("MoveIt pose reached.")
return_result = True
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self._move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
self._move_group.get_move_action().cancel_all_goals()
return return_result
def SetPoseWithRetry(self, target_pose, max_time=3):
for _ in range(max_time):
if self.SetPose(target_pose):
return True
return False
class RobotArm:
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
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("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.removeCollisionObject("table1")
def MoveToPose(self,X,Y,Z,x,y,z,w):
self.poseStamped = PoseStamped()
self.poseStamped.header.frame_id = 'base_link'
self.poseStamped.header.stamp = rospy.Time.now()
self.poseStamped.pose.position.x = X
self.poseStamped.pose.position.y = Y
self.poseStamped.pose.position.z = Z
self.poseStamped.pose.orientation.x = x
self.poseStamped.pose.orientation.y = y
self.poseStamped.pose.orientation.z = z
self.poseStamped.pose.orientation.w = w
self.moveGroup.moveToPose(self.poseStamped, 'gripper_link')
self.result = self.moveGroup.get_move_action().get_result()
def OriginReturn(self):
#Move to the origin
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, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
self.moveGroup.moveToJointPosition(joints, pose, 0.02)
while 1:
self.result = self.moveGroup.get_move_action().get_result()
if self.result.error_code.val == MoveItErrorCodes.SUCCESS:
break
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)
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"]
if not self.untuck:
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
else:
pose = [0.05, 0.29, -0.60, -0.51, 1.45, 0.52, 0.88, -1.97]
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 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)
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
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()
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 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 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 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
# 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"
]
# Lists of joint angles in the same order as in joint_names
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
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)
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, wait=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()
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()
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 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
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 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
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
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
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 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()
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()
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
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()
