class GripperClient: def __init__(self): self.move_group = MoveGroupInterface("gripper", "base_link") self.joint_names = ['l_gripper_finger_joint', 'r_gripper_finger_joint'] def open_gripper(self): value = [0.1, 0.1] self.__execu(value) # Plans the joints in joint_names to angles in pose def close_gripper(self): value = [0.0, 0.0] self.__execu(value) # Plans the joints in joint_names to angles in pose def __execu(self, value): self.move_group.moveToJointPosition(self.joint_names, value, 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() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: print 'gripper' else: # If you get to this point please search for: rospy.logerr( "Arm goal in state: %s", self.move_group.get_move_action().get_joint_values()) else: rospy.logerr("MoveIt! failure no result returned.")
def gotoInit(self): move_group = MoveGroupInterface("arm", "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # Plans the joints in joint_names to angles in pose init = self.trajActionGoal.trajectory.points[0].positions move_group.moveToJointPosition(joint_names, init, wait=False) 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("Moved to Init") 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.")
class moveRobot(object): def __init__(self): self.move_delay = 0.2 self.lastpose = [] self.lasttime = time.time() self.need_plan = True self.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" ] self.move_group = MoveGroupInterface("arm_with_torso", "base_link") def callback(self, data): if data.angles != self.lastpose: self.lastpose = data.angles self.lasttime = time.time() self.need_plan = True def check_pose_updates(self): if (not (self.need_plan and time.time() - self.lasttime > self.move_delay)): return self.need_plan = False pose = [0, 0, 0, 0, 0, 0, 0, 0] pose_data = self.lastpose for i in range(len(pose_data)): pose[i] = pose_data[i].data rospy.loginfo(rospy.get_caller_id() + " I heard %s", pose) self.move_group.moveToJointPosition(self.joint_names, pose, wait=True) print 'Moved To New Pose'
def main(): mgi = MoveGroupInterface("l_arm", "benderbase_link", None, False) #"bender/l_shoulder_pitch_link" tip_link = "benderl_wrist_pitch_link" joint_names = [ 'l_shoulder_pitch_joint', 'l_shoulder_roll_joint', 'l_shoulder_yaw_joint', 'l_elbow_pitch_joint', 'l_elbow_yaw_joint', 'l_wrist_pitch_joint' ] rospy.loginfo('STARTING TEST') count = 0 succ_plan = 0 home = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] mgi.moveToJointPosition(joint_names, home) rospy.loginfo('POSITION: home') rospy.sleep(2.0) #--------------------------------------------------------------------------------------------------------------------------------------------------- goal = PoseStamped() #Position goal.pose.position.x, goal.pose.position.y, goal.pose.position.z = 0.2, 0.1, 0.1 #simple_orientation = Quaternion(0.0,-0.70710678,0.0,0.70710678) #Orientation goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w = -0.0119682, -0.70163, -0.00910682, 0.712382 #-0.0148,-0.733 , 0.0, 0.678 goal.header.frame_id = "benderbase_link" #--------------------------------------------------------------------------------------------------------------------------------------------------- #------------------------------------------------Iterarion Params----------------------------------------------------------------------------------- dl = 0.03 #precision en metros (0.03) Xmin = -0.623244 #-0.05 Xmax = 0.615229 #0.8 Ymin = -0.829381 #-0.4 Ymax = -0.261919 #0.05 Zmin = 0.393434 #0.55 Zmax = 1.346654 #0.85 # ~17 horas worst case scenario #--------------------------------------------------------------------------------------------------------------------------------------------------- workspace = [] #matrix for saving coordinates and error codes for i in my_range(Xmin, Xmax, dl): for j in my_range(Ymin, Ymax, dl): for k in my_range(Zmin, Zmax, dl): goal.pose.position.x, goal.pose.position.y, goal.pose.position.z = i, j, k error_code = Move(mgi, tip_link, goal) if (error_code == 1): #Move(mgi,tip_link,goal)==1): succ_plan += 1 count += 1 workspace.append([error_code, i, j, k]) percentage = (succ_plan / count) * 100 rospy.loginfo('succeeded plannings: {0}/{1} ({2}%)'.format( succ_plan, count, percentage)) rospy.loginfo('TEST ENDED') with open( '/home/robotica/uchile_ws/pkgs/base_ws/bender_core/bender_test_config_v38/scripts/workspace.csv', 'wb') as f: writer = csv.writer(f) writer.writerows(workspace)
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 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)
def set_init_pose(self): #set init pose move_group = MoveGroupInterface("manipulator","base_link") #MoveGroupInterface のインスタンスの作成 planning_scene = PlanningSceneInterface("base_link") #PlanningSceneInterface のインスタンスの作成 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)#joint_names を pose に動かす move_group.get_move_action().wait_for_result() #wait result result = move_group.get_move_action().get_result() #result を代入 move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
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()
class GraspingClient(object): def __init__(self): self.move_group = MoveGroupInterface("arm", "base_link") 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 straight(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] 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 callback_posegoal(data): 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"] move_group = MoveGroupInterface("arm_with_torso", "base_link") pose = [0,0,0,0,0,0,0,0] pose_data = data.angles for i in range(len(pose_data)): pose[i] = pose_data[i].data rospy.loginfo(rospy.get_caller_id() + " I heard %s", pose) move_group.moveToJointPosition(joint_names,pose,wait=True) print ('Moved To New Pose')
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 _home_arm_planner(self): if self._prefix == 'left': rospy.sleep(5) else: move_group_jtas = MoveGroupInterface("upper_body", "base_link") move_group_jtas.setPlannerId("RRTConnectkConfigDefault") success = False while not rospy.is_shutdown() and not success: result = move_group_jtas.moveToJointPosition( self._body_joints, self._homed, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.logerr("_home_arm_planner completed ") success = True else: rospy.logerr( "_home_arm_planner: _home_arm_planner failed (%d)" % result.error_code.val) self._arms_homing = True self._ctl.api.MoveHome() self._ctl.api.InitFingers() self.home_arm_pub.publish(Bool(True)) rospy.sleep(2.0) self._arms_homing = False self._planner_homing = False
class MovoUpperBody: upper_body_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] homed = [ -1.5,-0.2,-0.175,-2.0,2.0,-1.24,-1.1, \ 1.5,0.2,0.15,2.0,-2.0,1.24,1.1,\ 0.35,0,0 ] tucked = [ -1.6,-1.4,0.4,-2.7,0.0,0.5,-1.7,\ 1.6,1.4,-0.4,2.7,0.0,-0.5, 1.7, 0.04, 0, 0] sidearms = [ -0.3 , -1.7 ,0.0 ,-1.7 ,1.8 ,-1.8 ,-1.1, \ 0.3 , 1.7 ,0.0 ,1.7 ,-1.8 ,1.8 ,1.1 ,\ .35, 0, -1] gripper_closed = 0.01 gripper_open = 0.165 def __init__(self): self.move_group = MoveGroupInterface("upper_body", "base_link") self.move_group.setPlannerId("RRTConnectkConfigDefault") self.lgripper = GripperActionClient('left') self.rgripper = GripperActionClient('right') def move(self, pose): success = False while not rospy.is_shutdown() and not success: result = self.move_group.moveToJointPosition(\ self.upper_body_joints, pose, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: success = True else: rospy.logerr("moveToJointPosition failed (%d)"\ %result.error_code.val) def untuck(self): rospy.loginfo("untucking the arms") self.lgripper.command(self.gripper_open) self.rgripper.command(self.gripper_open) self.move(self.homed) def tuck(self): self.move(self.tucked) self.lgripper.command(self.gripper_closed) self.rgripper.command(self.gripper_closed) def side(self): self.move(self.sidearms)
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
class TuckThread(Thread): def __init__(self, untuck=False): Thread.__init__(self) self.client = None self.untuck = untuck self.start() 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 stop(self): if self.client: self.client.get_move_action().cancel_goal() # 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("failed") sys.exit(0)
def tuck(): move_group = MoveGroupInterface("arm", "base_link") 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 = move_group.moveToJointPosition(joints, pose, 0.02)
class TrajectorySimulator(): def __init__(self): self.group = MoveGroupInterface("arm", "base_link") # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(15) print "============ Starting planning" def plan_to_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "gripper_link", tolerance=0.3, plan_only=True) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" return result.planned_trajectory def plan_to_jointspace_goal(self, pose): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" return "Success"
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 main(args): rospy.init_node('simple_move') move_group = MoveGroupInterface('manipulator', "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ 'shoulder_1_joint', 'shoulder_2_joint', 'elbow_1_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] poses, pose_name = get_pose(args.case, args.reset) for pose in poses: if rospy.is_shutdown(): break 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: log_output = pose_name + " Done!" rospy.loginfo(log_output) 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 TrajectorySimulator(): def __init__(self): self.group = MoveGroupInterface("arm", "base_link") # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(15) print "============ Starting planning" def plan_to_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "gripper_link", tolerance = 0.3, plan_only=True) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" return result.planned_trajectory def plan_to_jointspace_goal(self, pose): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" return "Success"
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)
class TuckThread(Thread): def __init__(self): Thread.__init__(self) self.client = None self.start() def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375) joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.client.moveToJointPosition(joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return def stop(self): if self.client: self.client.get_move_action().cancel_goal() # 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("failed") sys.exit(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
if __name__ == '__main__': try: global objectName, closeRC, closeLC object_topic = rospy.get_param('~object_topic') rospy.Subscriber(object_topic, obj, addObj) # cProfile to measure the performance (time) of the task. pr = cProfile.Profile() pr.enable() rightgripper.calibrate() leftgripper.calibrate() rightgripper.open() leftgripper.open() g.moveToJointPosition(jts_both, pos_high, max_velocity_scaling_factor=0.5, plan_only=False) # Awaits for the planning scene to build correctly time.sleep(10) print "There is", len(objectName), "object(s) detected on the table" # Sort the object list objectName, collisionObjects = sortObjects(objectName) # Pick and place loop i = 0 for collisionObject in collisionObjects: name = objectName[i] if collisionObject.primitive_poses[0].position.y < 0: side = 'right' else: side = 'left' print "READY TO PICK UP OBJECT", name
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 picknplace(): # Define initial parameters p = PlanningSceneInterface("base") g = MoveGroupInterface("both_arms", "base") gr = MoveGroupInterface("right_arm", "base") gl = MoveGroupInterface("left_arm", "base") leftgripper = baxter_interface.Gripper('left') leftgripper.calibrate() leftgripper.open() jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519] lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384] rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] # Clear planning scene p.clear() # Add table as attached object p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) # Move both arms to start state g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Loop to continue pick and place until all objects are cleared from table j=0 k=0 while locs_x: p.clear() # CLear planning scene of all collision objects # Attach table as attached collision object to base of baxter p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) xn = locs_x[0] yn = locs_y[0] zn = -0.06 thn = orien[0] sz = size[0] if thn > pi/4: thn = -1*(thn%(pi/4)) # Add collision objects into planning scene objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] for i in range(1,len(locs_x)): p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05) p.waitForSync() # Move both arms to pos2 (Right arm away and left arm on table) g.moveToJointPosition(jts_both, pos2, plan_only=False) # Move left arm to pick object and pick object pickgoal = PoseStamped() pickgoal.header.frame_id = "base" pickgoal.header.stamp = rospy.Time.now() pickgoal.pose.position.x = xn pickgoal.pose.position.y = yn pickgoal.pose.position.z = zn+0.1 pickgoal.pose.orientation.x = 1.0 pickgoal.pose.orientation.y = 0.0 pickgoal.pose.orientation.z = 0.0 pickgoal.pose.orientation.w = 0.0 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn) gl.moveToPose(pickgoal, "left_gripper", plan_only=False) pickgoal.pose.position.z = zn gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False) leftgripper.close() pickgoal.pose.position.z = zn+0.1 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Move both arms to pos1 g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Place object stleft = PoseStamped() stleft.header.frame_id = "base" stleft.header.stamp = rospy.Time.now() stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.55 stleft.pose.position.z = 0.1 stleft.pose.orientation.x = 1.0 stleft.pose.orientation.y = 0.0 stleft.pose.orientation.z = 0.0 stleft.pose.orientation.w = 0.0 # Stack objects (large cubes) if size if greater than some threshold if sz > 16.: stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.7 stleft.pose.position.z = -0.04+(k*0.05) k += 1 gl.moveToPose(stleft, "left_gripper", plan_only=False) leftgripper.open() stleft.pose.position.z = 0.3 gl.moveToPose(stleft, "left_gripper", plan_only=False)
class 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 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
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 TrajectoryGenerator(): # Wrapper class that facilitates trajectory planning. # Includes methods for planning to arbitrary joint-space goals # and end-effector pose goals. def __init__(self): self.group = MoveGroupInterface("arm", "base_link", plan_only=True) self.gripper = MoveGroupInterface("gripper", "base_link", plan_only=True) self.virtual_arm_state = RobotState() self.virtual_gripper_state = RobotState() # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(5) print "============ Starting planning" def generate_trajectories(self, motion_goals, **kwargs): approved_trajectories = list() supported_args = ("get_approval") for arg in kwargs.keys(): if arg not in supported_args: rospy.loginfo("generate_trajectories: unsupported argument: %s", arg) if type(motion_goals) is not type(list()): motion_goals = [motion_goals] for motion_goal in motion_goals: if motion_goal.goal_type is 'ee_pose': print "==== Virtual Start State: ", self.virtual_arm_state trajectory = self.plan_to_ee_pose_goal(motion_goal.pose) elif motion_goal.goal_type is "jointspace": trajectory = self.plan_to_jointspace_goal(motion_goal.state) elif motion_goal.goal_type is "gripper_posture": trajectory = self.plan_to_gripper_goal(motion_goal.posture) else: rospy.loginfo("Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'") sys.exit() try: kwargs["get_approval"] approved = self.get_user_approval() if approved: approved_trajectories.append(trajectory) self.update_virtual_state(trajectory) else: # Recur approved_trajectories += self.generate_trajectories(motion_goal, get_approval=True) except KeyError: pass returnable_approved_trajectories = copy.deepcopy(approved_trajectories) return returnable_approved_trajectories def get_user_approval(self): choice = 'not_set' options = {'y':True, 'r':False} while choice.lower() not in options.keys(): choice = raw_input("Add plan to execution queue? Answer 'y' for yes "\ " or 'r' for replan: ") return options[choice] def plan_to_ee_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "wrist_roll_link", tolerance = 0.02, plan_only=True, start_state = self.virtual_arm_state, planner_id = "PRMkConfigDefault") if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" self.trajectory = result.planned_trajectory.joint_trajectory return result.planned_trajectory def plan_to_jointspace_goal(self, state): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, state, plan_only=True, start_state=self.virtual_arm_state, planner_id = "PRMkConfigDefault") if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" self.trajectory = result.planned_trajectory.joint_trajectory return result.planned_trajectory def plan_to_gripper_goal(self, posture): joints = ["l_gripper_finger_joint", "r_gripper_finger_joint"] while not rospy.is_shutdown(): result = self.gripper.moveToJointPosition(joints, posture, plan_only=True, start_state=self.virtual_gripper_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Gripper Posture Plan Successful") self.trajectory = result.planned_trajectory.joint_trajectory return result.planned_trajectory def update_virtual_state(self, trajectory): # Sets joint names and sets position to final position of previous trajectory if len(trajectory.joint_trajectory.points[-1].positions) == 7: self.virtual_arm_state.joint_state.name = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] self.virtual_arm_state.joint_state.position = trajectory.joint_trajectory.points[-1].positions rospy.loginfo("Virtual Arm State Updated") print self.virtual_arm_state.joint_state.position elif len(trajectory.joint_trajectory.points[-1].positions) == 2: self.virtual_gripper_state.joint_state.name = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.virtual_gripper_state.joint_state.position = trajectory.joint_trajectory.points[-1].positions rospy.loginfo("Virtual Gripper State Updated") print self.virtual_gripper_state.joint_state.position def clear_virtual_arm_state(self): self.virtual_arm_state.joint_state.name = [] self.virtual_arm_state.joint_state.position = [] def clear_virtual_gripper_state(self): self.virtual_gripper_state.joint_state.names = [] self.virtual_gripper_state.joint_state.position = [] def get_virtual_arm_state(self): virtual_arm_state = copy.deepcopy(self.virtual_arm_state) return virtual_arm_state def get_virtual_gripper_state(self): virtual_gripper_state = copy.deepcopy(self.virtual_gripper_state) return virtual_gripper_state
class Robot(object): """ fetch max x = 0.96 (wrist roll joint) """ def __init__(self): rospy.loginfo("Waiting for Fetch ...") self.tuck_the_arm_joints_value = [ 0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0 ] self.stow_joints_value = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] self.move_group = MoveGroupInterface("arm_with_torso", "base_link") self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso") self.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" ] def move_to_pose(self, postion, quaternion, frame='base_link'): target_frame = 'wrist_roll_link' pose_stamped = PoseStamped() pose_stamped.header.frame_id = frame pose_stamped.header.stamp = rospy.Time.now() pose_stamped.pose = Pose( Point(postion[0], postion[1], postion[2]), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) if not rospy.is_shutdown(): self.move_group.moveToPose(pose_stamped, target_frame) result = self.move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Move success!") return True else: rospy.logerr("in state: %s", self.move_group.get_move_action().get_state()) return False else: rospy.logerr("MoveIt! failure no result returned.") return False def move_to_joints(self, joints_value): # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, joints_value, 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() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("move to joint Success!") 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.") def tuck_the_arm(self): self.move_to_joints(self.tuck_the_arm_joints_value) def stow(self): self.move_to_joints(self.stow_joints_value) def stop(self): """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()
class TrajectoryGenerator(): # Wrapper class that facilitates trajectory planning. # Includes methods for planning to arbitrary joint-space goals # and end-effector pose goals. def __init__(self): self.group = MoveGroupInterface("arm", "base_link") self.gripper = MoveGroupInterface("gripper", "base_link") self.virtual_state = RobotState() self.trajectory = None # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(5) print "============ Starting planning" def generate_trajectory(self, goal_poses): for goal_pose in goal_poses: if goal_pose.goal_type is "ee_pose": traj_result = self.plan_to_ee_pose_goal(goal_pose.pose) elif goal_pose.goal_type is "jointspace": traj_result = self.plan_to_jointspace_goal(goal_pose.state) elif goal_pose.goal_type is "gripper_posture": traj_result = self.plan_to_gripper_goal(goal_pose.state) else: rospy.loginfo("Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'") sys.exit() approved_trajectories.append(traj_result) return approved_trajectories def plan_to_ee_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "gripper_link", tolerance = 0.02, plan_only=True, start_state = self.virtual_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" self.trajectory = result.planned_trajectory.joint_trajectory return self.trajectory def plan_to_jointspace_goal(self, pose): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose, plan_only=True, start_state=self.virtual_state ) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" self.trajectory = result.planned_trajectory #.joint_trajectory return self.trajectory def plan_to_gripper_goal(self, posture): joints = ["gripper_link", "l_gripper_finger_link", "r_gripper_finger_link"] while not rospy.is_shutdown(): result = self.gripper.moveToJointPosition(joints, pose, plan_only=True, start_state=self.virtual_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Gripper Posture Plan Successful") self.trajectory = result.planned_trajectory.joint_trajectory return self.trajectory def update_virtual_state(self): # Sets joint names and sets position to final position of previous trajectory if self.trajectory == None: print "No trajectory available to provide a new virtual state." print "Update can only occur after trajectory has been planned." else: self.virtual_state.joint_state.name = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] self.virtual_state.joint_state.position = self.trajectory.points[-1].positions def clear_virtual_state(self): self.virtual_state.joint_state.joint_names = [] self.virtual_state.joint_state.position = [] def execute_trajectory(self, trajectory): execute_known_trajectory = rospy.ServiceProxy('execute_known_trajectory', ExecuteKnownTrajectory) result = execute_known_trajectory(trajectory)
class TrajectoryGenerator: # Wrapper class that facilitates trajectory planning. # Includes methods for planning to arbitrary joint-space goals # and end-effector pose goals. def __init__(self): self.group = MoveGroupInterface("arm", "base_link") self.gripper = MoveGroupInterface("gripper", "base_link") self.virtual_state = RobotState() self.trajectory = None # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(5) print "============ Starting planning" def generate_trajectory(self, goal_poses): for goal_pose in goal_poses: if goal_pose.goal_type is "ee_pose": traj_result = self.plan_to_ee_pose_goal(goal_pose.pose) elif goal_pose.goal_type is "jointspace": traj_result = self.plan_to_jointspace_goal(goal_pose.state) elif goal_pose.goal_type is "gripper_posture": traj_result = self.plan_to_gripper_goal(goal_pose.state) else: rospy.loginfo("Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'") sys.exit() approved_trajectories.append(traj_result) return approved_trajectories def plan_to_ee_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose( pose_target, "gripper_link", tolerance=0.02, plan_only=True, start_state=self.virtual_state ) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" self.trajectory = result.planned_trajectory.joint_trajectory return self.trajectory def plan_to_jointspace_goal(self, pose): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint", ] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose, plan_only=True, start_state=self.virtual_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" self.trajectory = result.planned_trajectory # .joint_trajectory return self.trajectory def plan_to_gripper_goal(self, posture): joints = ["gripper_link", "l_gripper_finger_link", "r_gripper_finger_link"] while not rospy.is_shutdown(): result = self.gripper.moveToJointPosition(joints, pose, plan_only=True, start_state=self.virtual_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Gripper Posture Plan Successful") self.trajectory = result.planned_trajectory.joint_trajectory return self.trajectory def update_virtual_state(self): # Sets joint names and sets position to final position of previous trajectory if self.trajectory == None: print "No trajectory available to provide a new virtual state." print "Update can only occur after trajectory has been planned." else: self.virtual_state.joint_state.name = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint", ] self.virtual_state.joint_state.position = self.trajectory.points[-1].positions def clear_virtual_state(self): self.virtual_state.joint_state.joint_names = [] self.virtual_state.joint_state.position = [] def execute_trajectory(self, trajectory): execute_known_trajectory = rospy.ServiceProxy("execute_known_trajectory", ExecuteKnownTrajectory) result = execute_known_trajectory(trajectory)
class TrajectorySimulator(): def __init__(self): # Instantiate a RobotCommander object. # This object is an interface to the robot as a whole. #robot = moveit_commander.RobotCommander() # Instantiate a MoveGroupCommander object. # This object is an interface to one 'group' of joints, # in our case the joints of the arm. self.group = MoveGroupInterface("arm", "base_link") # Create DisplayTrajectory publisher to publish trajectories # for RVIZ to visualize. # self.display_trajectory_publisher = rospy.Publisher( #"/move_group/display_planned_path", #moveit_msgs.msg.DisplayTrajectory) # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(15) print "============ Starting tutorial " def get_state_info(self): # Let's get some basic information about the robot. # This info may be helpful for debugging. # Get name of reference frame for this robot print "============ Reference frame: %s" % self.group.get_planning_frame() # Get name of end-effector link for this group print "============ Reference frame: %s" % self.group.get_end_effector_link() # List all groups defined on the robot print "============ Robot Groups:" print self.robot.get_group_names() # Print entire state of the robot print "============ Printing robot state" print self.robot.get_current_state() print "============" def plan_to_pose_goal(self, goal): ### Planning to a Pose Goal # Let's plan a motion for this group to a desired pose for the # end-effector print "============ Generating plan 1" # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "gripper_link", tolerance = 0.3, plan_only=False) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" return # Now we call the planner to compute the plan and visualize it if successful. # We are just planning here, not asking move_group to actually move the robot. print "============ Waiting while RVIZ displays plan1..." rospy.sleep(5) print "============ Goal achieved" # group.plan() automatically asks RVIZ to visualize the plan, # so we need not explicitly ask RVIZ to do this. # For completeness of understanding, however, we make this # explicit request below. The same trajectory should be displayed # a second time. print "============ Visualizing plan1" """ display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state() display_trajectory.trajectory.append(plan1) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while plan1 is visualized (again)..." rospy.sleep(5) """ def plan_to_jointspace_goal(self, pose): # Let's set a joint-space goal and visualize it in RVIZ # A joint-space goal differs from a pose goal in that # a goal for the state of each joint is specified, rather # than just a goal for the end-effector pose, with no # goal specified for the rest of the arm. joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose, 0.1) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" rospy.sleep(5) return "Success"
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 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
# Lists of joint angles in the same order as in joint_names disco_poses = [[0.0, 1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]] disco_poses1 = [[0.0, 1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0], [0.133, 0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0], [0.266, -0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0], [0.385, -1.5, 1.1, -3.0, -0.5, -3.0, -1.0, -3.0], [0.266, -0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0], [0.133, 0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0], [0.0, 1.5, -0.6, 3.0, 1.0, 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:
class GraspingClient(object): def __init__(self, sim=False): # Define planning groups self.scene = PlanningSceneInterface("base_link") self.dof = rospy.get_param('~jaco_dof') self.move_group = MoveGroupInterface("upper_body", "base_link") self.rmove_group = MoveGroupInterface("right_arm", "base_link") planner_id = "RRTConnectkConfigDefault" self.move_group.setPlannerId(planner_id) self.rmove_group.setPlannerId(planner_id) self.objects_heights = [0.122, 0.240] self.objects_heights_tolerances = [0.02, 0.03] # Define joints and positions for 6 and 7 DOF if "6dof" == self.dof: self._upper_body_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint", "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint" ] self.tucked = [ -2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571, 0.0371, 0.0, 0.0 ] self.constrained_stow = [ 2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56, 0.09, -0.15, 2.06, 0.42, 0.0, 0.0 ] self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06] #self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32] elif "7dof" == self.dof: self._upper_body_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint" ] self.tucked = [ -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7, 0.0, -0.5, 1.7, 0.04, 0, 0 ] self.constrained_stow = [ -2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0 ] self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0] else: rospy.logerr("DoF needs to be set 6 or 7, aborting demo") return # Define the MoveIt pickplace pipeline self.pickplace = None self.pickplace = PickPlaceInterface("left_side", "left_gripper", verbose=True) self.pickplace.planner_id = planner_id self.pick_result = None self.place_result = None self._listener = tf.TransformListener() self._lgripper = GripperActionClient('left') find_grasp_planning_topic = "grasp_planner/plan" rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic) self.find_grasp_planning_client = actionlib.SimpleActionClient( find_grasp_planning_topic, GraspPlanningAction) self.find_grasp_planning_client.wait_for_server() rospy.loginfo("...connected") self.scene.clear() # This is a simulation so need to adjust gripper parameters if sim: self._gripper_closed = 0.96 self._gripper_open = 0.00 else: self._gripper_closed = 0.01 self._gripper_open = 0.165 def __del__(self): self.scene.clear() def add_objects_to_keep(self, obj): self._objs_to_keep.append(obj) def clearScene(self): self.scene.clear() def calculateGraspForObject(self, object_to_grasp, gripper): goal = GraspPlanningGoal() goal.object = object_to_grasp goal.gripper = gripper self.find_grasp_planning_client.send_goal(goal) self.find_grasp_planning_client.wait_for_result(rospy.Duration(5.0)) return self.find_grasp_planning_client.get_result( ).grasps #moveit_msgs/Grasp[] def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, retries=1, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, retries=1, scene=self.scene) self.place_result = place_result return success def goto_tuck(self): # remove previous objects while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition( self._upper_body_joints, self.tucked, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def goto_plan_grasp(self): while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition( self._upper_body_joints, self.constrained_stow, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def left_arm_constrained_stow(self): c1 = Constraints() c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.stamp = rospy.get_rostime() c1.orientation_constraints[0].header.frame_id = "base_link" c1.orientation_constraints[0].link_name = "left_ee_link" c1.orientation_constraints[0].orientation.w = 1.0 c1.orientation_constraints[ 0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2 c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28 c1.orientation_constraints[0].weight = 1.0 while not rospy.is_shutdown(): result = self.lmove_group.moveToJointPosition( self._left_arm_joints, self.larm_const_stow, 0.05, path_constraints=c1, planning_time=120.0) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def right_arm_constrained_stow(self): # c1 = Constraints() # c1.orientation_constraints.append(OrientationConstraint()) # c1.orientation_constraints[0].header.stamp = rospy.get_rostime() # c1.orientation_constraints[0].header.frame_id = "base_link" # c1.orientation_constraints[0].link_name = "right_ee_link" # c1.orientation_constraints[0].orientation.w=1.0 # c1.orientation_constraints[0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link # c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2 # c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28 # c1.orientation_constraints[0].weight = 1.0 while not rospy.is_shutdown(): result = self.rmove_group.moveToJointPosition( self._right_arm_joints, self.rarm_const_stow, 0.05, planning_time=120.0) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def open_gripper(self): self._lgripper.command(self._gripper_open, block=True) def close_gripper(self): self._lgripper.command(self._gripper_closed, block=True)