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 setup_scene(): scene = PlanningSceneInterface('base_link') scene.removeCollisionObject('box') scene.addCube('box', 0.25, -0.45, 0.1, 0.125) pick_place = PickPlaceInterface('xarm5', 'gripper', verbose=True) grasp = Grasp() # fill in g # setup object named object_name using PlanningSceneInterface pick_place.pickup('box', [grasp], support_name='supporting_surface') place_loc = PlaceLocation() # fill in l pick_place.place('box'[place_loc], goal_is_eef=True, support_name='supporting_surface')
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
def __init__(self): self.moving_mode = False self.plan_only = False self.prev_joy_buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] self.joy_subscriber = rospy.Subscriber("/fetch_joy", Joy, self.joy_callback) self.joints_subscriber = rospy.Subscriber("/joint_states", JointState, self.joints_states_callback) self.rgbimage_subscriber = rospy.Subscriber("/head_camera/depth/image", Image, self.depthimage_callback) self.depthimage_subscriber = rospy.Subscriber( "/head_camera/depth/image", Image, self.rgbimage_callback) self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_reset_pos", Empty, self.arm_reset_callback) self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_start_pos", Empty, self.arm_start_callback) self.arm_effort_pub = rospy.Publisher( "/arm_controller/weightless_torque/command", JointTrajectory, queue_size=2) self.gripper_client = actionlib.SimpleActionClient( "gripper_controller/gripper_action", GripperCommandAction) self.arm_cartesian_twist_pub = rospy.Publisher( "/arm_controller/cartesian_twist/command", Twist, queue_size=2) self.head_point_client = actionlib.SimpleActionClient( "head_controller/point_head", PointHeadAction) self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only=self.plan_only) planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def setBoundaries(): ''' This is a fix for the FETCH colliding with itself Define ground plane This creates objects in the planning scene that mimic the ground If these were not in place gripper could hit the ground ''' planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects rospy.loginfo("Add collision objects") self.scene.removeCollisionObject("my_front_ground") self.scene.removeCollisionObject("my_back_ground") self.scene.removeCollisionObject("my_right_ground") self.scene.removeCollisionObject("my_left_ground") self.scene.removeCollisionObject("box_1") self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.scene.addBox("box_1", 0.44, 0.43, 0.68, 0.7, -0.4, .34) self.scene.waitForSync() def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def zero(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
def move(): rospy.init_node("move") move_group = MoveGroupInterface("arm_with_torso", "base_link") #planning scene setup planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) #move head camera client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction) rospy.loginfo("Waiting for head_controller...") client.wait_for_server() goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = "base_link" goal.target.point.x = 1.2 goal.target.point.y = 0.0 goal.target.point.z = 0.0 goal.min_duration = rospy.Duration(1.0) client.send_goal(goal) client.wait_for_result() #arm setup move_group = MoveGroupInterface("arm", "base_link") #set arm initial position joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class 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
class PlanningSceneClient(object): '''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''' def __init__(self): 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 close(self): 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")
def picknplace(): # Define initial parameters p = PlanningSceneInterface("base") g = MoveGroupInterface("both_arms", "base") gr = MoveGroupInterface("right_arm", "base") gl = MoveGroupInterface("left_arm", "base") leftgripper = baxter_interface.Gripper('left') leftgripper.calibrate() leftgripper.open() jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519] lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384] rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] # Clear planning scene p.clear() # Add table as attached object p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) # Move both arms to start state g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Loop to continue pick and place until all objects are cleared from table j=0 k=0 while locs_x: p.clear() # CLear planning scene of all collision objects # Attach table as attached collision object to base of baxter p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) xn = locs_x[0] yn = locs_y[0] zn = -0.06 thn = orien[0] sz = size[0] if thn > pi/4: thn = -1*(thn%(pi/4)) # Add collision objects into planning scene objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] for i in range(1,len(locs_x)): p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05) p.waitForSync() # Move both arms to pos2 (Right arm away and left arm on table) g.moveToJointPosition(jts_both, pos2, plan_only=False) # Move left arm to pick object and pick object pickgoal = PoseStamped() pickgoal.header.frame_id = "base" pickgoal.header.stamp = rospy.Time.now() pickgoal.pose.position.x = xn pickgoal.pose.position.y = yn pickgoal.pose.position.z = zn+0.1 pickgoal.pose.orientation.x = 1.0 pickgoal.pose.orientation.y = 0.0 pickgoal.pose.orientation.z = 0.0 pickgoal.pose.orientation.w = 0.0 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn) gl.moveToPose(pickgoal, "left_gripper", plan_only=False) pickgoal.pose.position.z = zn gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False) leftgripper.close() pickgoal.pose.position.z = zn+0.1 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Move both arms to pos1 g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Place object stleft = PoseStamped() stleft.header.frame_id = "base" stleft.header.stamp = rospy.Time.now() stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.55 stleft.pose.position.z = 0.1 stleft.pose.orientation.x = 1.0 stleft.pose.orientation.y = 0.0 stleft.pose.orientation.z = 0.0 stleft.pose.orientation.w = 0.0 # Stack objects (large cubes) if size if greater than some threshold if sz > 16.: stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.7 stleft.pose.position.z = -0.04+(k*0.05) k += 1 gl.moveToPose(stleft, "left_gripper", plan_only=False) leftgripper.open() stleft.pose.position.z = 0.3 gl.moveToPose(stleft, "left_gripper", plan_only=False)
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) self.robot = moveit_commander.RobotCommander() self.model_pose = [0.0, 0.0, 0.0] self.obstacle_pose = [0.0, 0.0, 0.0] self.set_forbidden() self.set_init_pose() def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.planning_scene.removeCollisionObject("demo_cube") self.planning_scene.removeCollisionObject("obstacle") print dir(self.planning_scene) import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) def set_init_pose(sefl): #set init pose move_group = MoveGroupInterface("manipulator", "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() move_group.get_move_action().cancel_all_goals() def callback(self, data): x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z self.model_pose = [x, y, z] x = data.pose[2].position.x y = data.pose[2].position.y z = data.pose[2].position.z self.obstacle_pose = [x, y, z] def start_subscriber(self): rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) def target(self): rate = rospy.Rate(1) rospy.sleep(1) while not rospy.is_shutdown(): self.planning_scene.removeCollisionObject("demo_cube") self.planning_scene.removeCollisionObject("obstacle") self.planning_scene.addCube("demo_cube", 0.06, self.model_pose[0], self.model_pose[1], self.model_pose[2]) self.planning_scene.addBox("obstacle", 0.540614, 0.083603, 0.54373, self.obstacle_pose[0], self.obstacle_pose[1], self.obstacle_pose[2]) print self.model_pose group = moveit_commander.MoveGroupCommander("manipulator") #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = pose.orientation.x pose_target.orientation.y = pose.orientation.y pose_target.orientation.z = pose.orientation.z pose_target.orientation.w = pose.orientation.w #pose_target.position.x = pose.position.x #pose_target.position.y = pose.position.y #pose_target.position.z = pose.position.z pose_target.position.x = self.model_pose[0] pose_target.position.y = self.model_pose[1] pose_target.position.z = self.model_pose[2] + 0.05 group.set_pose_target(pose_target) group.go() rospy.sleep(1)
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) #ノードの初期化 reaching ていう名前 self.robot = moveit_commander.RobotCommander() # インスタンスの作成 self.model_pose = [0.0, 0.0, 0.0] #モデルの姿勢 self.obstacle_pose = [0.0, 0.0, 0.0] #障害物の位置 self.subsc_pose = [0.0, 0.0, 0.0] #購読した座標 self.subsc_orientation = [0.0, 0.0, 0.0, 1.0] #購読した四元数 self.set_forbidden() #禁止エリアの初期化 self.set_init_pose() #位置姿勢の初期化 def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface( "base_link") #PlanningSceneInterface のインスタンスの作成 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, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.0, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.0, -1.0) #座標に立方体を挿入 self.planning_scene.removeCollisionObject("demo_cube") self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除 print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示 import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得 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 callback(self, data): #トピックにデータが追加されるたびに呼ばれる x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z self.model_pose = [x, y, z] #modelの姿勢を代入 # x = data.pose[2].position.x # y = data.pose[2].position.y # z = data.pose[2].position.z # self.obstacle_pose = [x,y,z] #障害物の姿勢を代入 def callbacksub(self, data): x = data.x y = data.y z = data.z self.subsc_pose = [x, y, z] #購読した座標を代入 def callbackunity(self, data): x = data.pose.position.y y = -data.pose.position.x z = data.pose.position.z self.subsc_pose = [x, y, z] #unity からの座標 座標変換も行っている x = data.pose.orientation.x y = data.pose.orientation.y z = data.pose.orientation.z w = data.pose.orientation.w self.subsc_orientation = [x, y, z, w] #unity からの四元数 def start_subscriber(self): #購読をスタート rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) # /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ rospy.Subscriber("/UR/reaching/pose", Vector3, self.callbacksub) rospy.Subscriber("/unity/target", PoseStamped, self.callbackunity) #unityから配信しているトピックを購読 def target(self): rate = rospy.Rate(1) #Rateクラスのインスタンス rateを1で rospy.sleep(1) #1秒スリープ while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、 self.planning_scene.removeCollisionObject("demo_cube") #オブジェクトの削除 self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除 self.planning_scene.addCube( "demo_cube", 0.06, self.model_pose[0], self.model_pose[1], self.model_pose[2]) #一辺が0.06の正方形をmodel_pose(座標)に追加 #self.planning_scene.addBox("obstacle", 0.5,0.5,0.5,0.5,0.5,0)#---の大きさの箱をobstacle_pose(座標)に追加 print self.model_pose #model_poseを表示 group = moveit_commander.MoveGroupCommander( "manipulator") #MoveGroupCommander クラスのインスタンス #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢 pose_target = geometry_msgs.msg.Pose( ) #geometry_msgs.pose のインスタンス これに入れるとトピックに反映される? pose_target.orientation.x = self.subsc_orientation[0] pose_target.orientation.y = self.subsc_orientation[1] pose_target.orientation.z = self.subsc_orientation[2] pose_target.orientation.w = self.subsc_orientation[ 3] #トピックに 四元数を代入 #pose_target.position.x = pose.position.x #pose_target.position.y = pose.position.y #pose_target.position.z = pose.position.z #pose_target.position.x = self.model_pose[0] # #pose_target.position.y = self.model_pose[1] # #pose_target.position.z = self.model_pose[2]+0.05 #トピックに 座標を代入 pose_target.position.x = self.subsc_pose[0] # pose_target.position.y = self.subsc_pose[1] # pose_target.position.z = self.subsc_pose[2] #トピックに 座標を代入 group.set_pose_target(pose_target) #ターゲットをセット group.go() #移動 rospy.sleep(1) #1秒スリープ
# of are itself & the floor. if __name__ == '__main__': rospy.init_node("simple_disco") # Create move group interface for a fetch robot move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TF joint names joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # Lists of joint angles in the same order as in joint_names 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],
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) self.model_pose = [0.0, 0.0, 0.0] self.set_init_pose() #self.set_forbidden() def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.planning_scene.removeCollisionObject("demo_cube") def set_init_pose(self): #set init pose move_group = MoveGroupInterface("arm_with_torso", "base_link") #set arm initial position joints = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] #pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] pose = [0.385, -1.0, -1.0, 0.0, 1.0, 0.0, 1.5, 0.0] #pose = [0.385, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] result = move_group.moveToJointPosition(joints, pose, 0.02) def target(self): rate = rospy.Rate(1) rospy.sleep(1) ls = tf.TransformListener() while not rospy.is_shutdown(): print self.model_pose try: (trans, rot) = ls.lookupTransform('/base_link', '/object', rospy.Time(0)) print "tf base_link to test", trans group = moveit_commander.MoveGroupCommander("arm_with_torso") #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = pose.orientation.x pose_target.orientation.y = pose.orientation.y pose_target.orientation.z = pose.orientation.z pose_target.orientation.w = pose.orientation.w pose_target.position.x = trans[0] pose_target.position.y = trans[1] pose_target.position.z = trans[2] + 0.3 group.set_pose_target(pose_target) group.go() except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "e" rospy.sleep(1)
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) self.robot = moveit_commander.RobotCommander() self.model_pose = [0.0,0.0,0.0] self.set_forbidden() self.set_init_pose() def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) def set_init_pose(sefl): #set init pose move_group = MoveGroupInterface("manipulator","base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() move_group.get_move_action().cancel_all_goals() def callback(self,data): #print data.markers[0].pose.position x = data.markers[0].pose.position.x y = data.markers[0].pose.position.y z = data.markers[0].pose.position.z self.model_pose = [x,y,z] def start_subscriber(self): rospy.Subscriber("/visualization_marker_array", MarkerArray, self.callback) def publish_tf(self): br = tf.TransformBroadcaster() br.sendTransform((self.model_pose[0], self.model_pose[1], self.model_pose[2]), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "object", "depth_camera_frame") def target(self): rate = rospy.Rate(1) rospy.sleep(1) ls = tf.TransformListener() while not rospy.is_shutdown(): self.publish_tf() try: (trans,rot) = ls.lookupTransform('/base_link', '/object', rospy.Time(0)) print "tf base_link to test",trans group = moveit_commander.MoveGroupCommander("manipulator") #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = pose.orientation.x pose_target.orientation.y = pose.orientation.y pose_target.orientation.z = pose.orientation.z pose_target.orientation.w = pose.orientation.w pose_target.position.x = trans[0] pose_target.position.y = trans[1] pose_target.position.z = trans[2]+0.2 group.set_pose_target(pose_target) group.go() except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "e" rospy.sleep(1) self.set_init_pose()
def main(): rospy.init_node('move_group_python_path_follow', anonymous=True) 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) moveit_commander.roscpp_initialize(sys.argv) move_group = MoveGroupInterface("arm", "base_link") robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("arm") print "============ Robot Groups:" print robot.get_group_names() print "============ Printing robot state" print robot.get_current_state() print "============" gripper_frame = 'wrist_roll_link' gripper_poses = list() radius = 0.2 for theta in np.arange(-pi, pi, 0.5 * pi): gripper_poses.append( Pose( Point(0.75, radius * np.sin(theta), 0.6 + radius * np.cos(theta)), Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)))) gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' gripper_pose_stamped.header.stamp = rospy.Time.now() gripper_pose_stamped.pose = gripper_poses[0] move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Success!") 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()) trajectory = RobotTrajectory() (plan, fraction) = group.compute_cartesian_path(gripper_poses, 0.01, 0.0) group.execute(plan) move_group.get_move_action().cancel_all_goals()
def move_corner(self, x, y): position = self.cal_position.get_base_position_from_pix(x, y) position[0] = position[0] - 0.20 move_group = MoveGroupInterface("arm_with_torso", "base_link") planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # This is the wrist link not the gripper itself gripper_frame = 'wrist_roll_link' pose = Pose(Point(position[0], position[1], position[2]), Quaternion(0, 0, 0, 1)) # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' # Finish building the Pose_stamped message # If the message stamp is not current it could be ignored gripper_pose_stamped.header.stamp = rospy.Time.now() # Set the message pose gripper_pose_stamped.pose = pose # Move gripper frame to the pose specified move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Hello there!") 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.") time.sleep(1) 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" ] joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] move_group.moveToJointPosition(joint_names, joints_value, 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("pose 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.")
if (rospy.Time.now() - t).to_sec() > 0.2: rospy.sleep(0.1) continue item_translation, item_orientation = tf_listener.lookupTransform( '/base_link', item_frame, t) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue gripper_goal.command.position = 0.15 gripper.send_goal(gripper_goal) gripper.wait_for_result(rospy.Duration(1.0)) print('Item: ' + str(item_translation)) scene.addCube('item', 0.05, item_translation[0], item_translation[1], item_translation[2]) p.position.x = item_translation[0] - 0.01 - 0.06 p.position.y = item_translation[1] p.position.z = item_translation[2] + 0.04 + 0.14 p.orientation = Quaternion(*quaternion_from_euler(0, 1.2, 0)) arm.set_pose_target(p) arm.go(True) gripper_goal.command.position = 0 gripper.send_goal(gripper_goal) gripper.wait_for_result(rospy.Duration(2.0)) scene.removeAttachedObject('item') clear_octomap()
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
X, Q = get_pose_from_msg(object_pose) print(X) print(Q) # 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.removeCollisionObject("table") 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) table_height = 0.7 planning_scene.addCube("table", table_height, X[0] + 0.3, X[1], 0.5 * table_height) print('created secene') # Create move group interface for a fetch robot move_group = MoveGroupInterface("arm_with_torso", "base_link") move_group_gripper = MoveGroupInterface("gripper_custom", "gripper_link") gripper_frame = 'wrist_roll_link'
class FetchTrajectoryRecorderNode: def __init__(self, node_name='fetch_trajectory_recorder_node'): rospy.init_node(node_name) # Create move group interface for a fetch robot self.move_group = MoveGroupInterface("arm_with_torso", "base_link") self.gripper_names = [ 'l_gripper_finger_joint', 'r_gripper_finger_joint' ] self.ee_name = 'wrist_roll_link' self.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ] self.tf_listener = tf.TransformListener() #set home as the initial state self.home_gripper_values = [0.08, 0.08] self.home_joint_values = [ -1.54, -0.036, -1.055, -1.53, -0.535, -1.42, 2.398 ] self.gripper_values = [] self.joint_values = [] self.received_joint_poses = [] self.temp_joint_values = home_joint_values self.temp_gripper_values = home_gripper_values self.gripper_values.append(home_gripper_values) self.joint_values.append(home_joint_values) rospy.Subscriber('joint_states', JointState, self.arm_trajectory_record_cb) #TODO: change this topic name as gripper related topics # rospy.Subscriber('joint_states', JointState, self.gripper_record_cb) # 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) #record the arm joint values def arm_trajectory_record_cb(self, msg): import IPython IPython.embed() for name, position in zip(msg.name, msg.position): if name in self.joint_names: #if the new position is different from the previous one if position != temp_joint_values: self.joint_values.append(position) self.temp_joint_values = position #go back to initial position as the last state self.joint_values.append(home_joint_values) # for i, joint_name in enumerate(msg.name): # if joint_name in self.joint_names: # idx = self.joint_names.index(joint_name) # self.received_joint_poses[idx] = msg.position[i] # else: # self.joint_names.append(joint_name) # self.received_joint_poses.append(msg.position[i]) #record the gripper values def gripper_record_cb(self, msg): for name, position in zip(msg.name, msg.position): if name in self.gripper_names: if position != temp_gripper_values: self.gripper_values.append(position) self.temp_gripper_values = position #go back to initial position as the last state self.gripper_values.append(home_gripper_values) #play back the trajetory just recorded #TODO: write a gui to trigger this function to play back #Otherwise, we will keep recording the joint values and do not move def play_back(self, joint_values, gripper_values): for joint in joint_values: if rospy.is_shutdown(): break # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, joint, 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("Play Back ...") 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 PoseController(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) self.angle = np.zeros(FILTER_LENGTH) self.last_trigger_time = rospy.Time.now() rospy.Subscriber(AUDIO_TOPIC, SoundSourceAngle, self.SetPoseWithRetry) rospy.spin() def SetPoseWithRetry(self, msg, max_time=3): if msg.is_valid: direction = 0 (direction, self.angle) = TemporalMedian(msg.angle, self.angle) soulder_pan = round((direction / 180.0) * np.pi, 2) target_pose = { 'torso_lift_joint': 0.05287206172943115, 'shoulder_pan_joint': soulder_pan, 'shoulder_lift_joint': 0.0, 'upperarm_roll_joint': -np.pi, 'elbow_flex_joint': 0.0, 'forearm_roll_joint': np.pi, 'wrist_flex_joint': -1.5, 'wrist_roll_joint': 0.0, } if rospy.Time.now() - self.last_trigger_time > rospy.Duration(2): rospy.loginfo("Shoulder pan") rospy.loginfo(soulder_pan) for _ in range(max_time): if self.SetPose(target_pose): return True return False 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("Done!") 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() self.last_trigger_time = rospy.Time.now() return return_result
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects rospy.loginfo("get new grasps") goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.removeCollisionObject("my_front_ground") self.scene.removeCollisionObject("my_back_ground") self.scene.removeCollisionObject("my_right_ground") self.scene.removeCollisionObject("my_left_ground") self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TODO: ADD BOX self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z # added + .1 obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def zero(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
def main(): rospy.init_node("robo_highfive") ### Move it Init stuff ------------------------------------------------------------ # Create move group interface for a fetch robot move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TF joint names joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] protoFile = "hand/pose_deploy.prototxt" weightsFile = "hand/pose_iter_102000.caffemodel" nPoints = 22 POSE_PAIRS = [[0, 1], [1, 2], [2, 3], [3, 4], [0, 5], [5, 6], [6, 7], [7, 8], [0, 9], [9, 10], [10, 11], [11, 12], [0, 13], [13, 14], [14, 15], [15, 16], [0, 17], [17, 18], [18, 19], [19, 20]] threshold = 0.2 ### Hand Detection init stuff --------------------------------------------------------------- input_source = "asl.mp4" cap = cv2.VideoCapture(input_source) hasFrame, frame = cap.read() frameWidth = frame.shape[1] frameHeight = frame.shape[0] aspect_ratio = frameWidth / frameHeight inHeight = 368 inWidth = int(((aspect_ratio * inHeight) * 8) // 8) vid_writer = cv2.VideoWriter('output.avi', cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'), 15, (frame.shape[1], frame.shape[0])) net = cv2.dnn.readNetFromCaffe(protoFile, weightsFile) k = 0 highFive_Threshhold = [30, 30, 30, 30, 30] ### High Five Detection ------------------------------------------------------------------------ while 1: k += 1 t = time.time() hasFrame, frame = cap.read() frameCopy = np.copy(frame) if not hasFrame: cv2.waitKey() break inpBlob = cv2.dnn.blobFromImage(frame, 1.0 / 255, (inWidth, inHeight), (0, 0, 0), swapRB=False, crop=False) net.setInput(inpBlob) output = net.forward() print("forward = {}".format(time.time() - t)) # Empty list to store the detected keypoints points = [] finger1 = None finger2 = None highFive = True for i in range(nPoints): # confidence map of corresponding body's part. probMap = output[0, i, :, :] probMap = cv2.resize(probMap, (frameWidth, frameHeight)) # Find global maxima of the probMap. minVal, prob, minLoc, point = cv2.minMaxLoc(probMap) if prob > threshold: cv2.circle(frameCopy, (int(point[0]), int(point[1])), 6, (0, 255, 255), thickness=-1, lineType=cv2.FILLED) cv2.putText(frameCopy, "{}".format(i), (int(point[0]), int(point[1])), cv2.FONT_HERSHEY_SIMPLEX, .8, (0, 0, 255), 2, lineType=cv2.LINE_AA) # if i == 8: # finger1 = [int(point[0]), int(point[1])] # if i == 12: # finger2 = [int(point[0]), int(point[1])] # #distance = np.sqrt((finger1[0] - finger2[0]) ** 2 + (finger1[1] - finger2[1]) ** 2) #print(distance) # Add the point to the list if the probability is greater than the threshold points.append((int(point[0]), int(point[1]))) else: points.append(None) if (i != 21): highFive = False print("highFive??", highFive) if highFive: pinky = np.sqrt((points[20][0] - points[18][0])**2 + (points[20][1] - points[18][1])**2) ring = np.sqrt((points[16][0] - points[14][0])**2 + (points[16][1] - points[14][1])**2) bird = np.sqrt((points[12][0] - points[10][0])**2 + (points[12][1] - points[10][1])**2) index = np.sqrt((points[8][0] - points[6][0])**2 + (points[8][1] - points[6][1])**2) thumb = np.sqrt((points[4][0] - points[2][0])**2 + (points[4][1] - points[2][1])**2) # print("pinky: ", pinky) # print("ring: ", ring) # print("bird: ", bird) # print("index: ", index) # print("thumb: ", thumb) finger_dists = [pinky, ring, bird, index, thumb] print("Finger Distances: ", finger_dists) for j in range(5): if finger_dists[j] < highFive_Threshhold[j]: print("False Positive!", j) break print() # Draw Skeleton for pair in POSE_PAIRS: partA = pair[0] partB = pair[1] if points[partA] and points[partB]: cv2.line(frame, points[partA], points[partB], (0, 255, 255), 2, lineType=cv2.LINE_AA) cv2.circle(frame, points[partA], 5, (0, 0, 255), thickness=-1, lineType=cv2.FILLED) cv2.circle(frame, points[partB], 5, (0, 0, 255), thickness=-1, lineType=cv2.FILLED) print("Time Taken for frame = {}".format(time.time() - t)) # cv2.putText(frame, "time taken = {:.2f} sec".format(time.time() - t), (50, 50), cv2.FONT_HERSHEY_COMPLEX, .8, (255, 50, 0), 2, lineType=cv2.LINE_AA) # cv2.putText(frame, "Hand Pose using OpenCV", (50, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (255, 50, 0), 2, lineType=cv2.LINE_AA) cv2.imshow('Output-Skeleton', frame) # cv2.imwrite("video_output/{:03d}.jpg".format(k), frame) key = cv2.waitKey(1) if key == 27: break print("total = {}".format(time.time() - t)) vid_writer.write(frame) vid_writer.release()
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) self.robot = moveit_commander.RobotCommander() # self.model_pose = [0.0,0.2,0.2 , 0,0,0,1] self.model_pose = [0.0, 0.3, 0.1, 0, 0, 0, 1] self.obstacle_pose = [0.0, 0.0, 0.0] self.set_forbidden() self.set_init_pose() def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_ground") self.planning_scene.addCube("my_ground", 2, 0, 0, -1.04) self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5) self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5) #self.planning_scene.addCube("demo_cube", 0.06,0,0.3,0) print dir(self.planning_scene) import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) def set_init_pose(sefl): #set init pose move_group = MoveGroupInterface("manipulator", "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] pose = [-1.26, -0.64, -2.44, -0.66, 1.56, 0.007] move_group.moveToJointPosition(joint_names, pose, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() move_group.get_move_action().cancel_all_goals() def callback(self, data): x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z def start_subscriber(self): rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) def reaching_pose(self): print self.model_pose group = moveit_commander.MoveGroupCommander("manipulator") #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = self.model_pose[3] pose_target.orientation.y = self.model_pose[4] pose_target.orientation.z = self.model_pose[5] pose_target.orientation.w = self.model_pose[6] pose_target.position.x = self.model_pose[0] pose_target.position.y = self.model_pose[1] pose_target.position.z = self.model_pose[2] group.set_pose_target(pose_target) group.go() #raw_input("Enter -->") def gripper_close(self): print("Gripper_Close") gripper_pose = Char() gripper_pose = 255 pub.publish(gripper_pose) #raw_input("Enter -->") def gripper_open(self): print("Gripper_Open") gripper_pose = Char() gripper_pose = 0 pub.publish(gripper_pose) #raw_input("Enter -->") def move_45(self): rate = rospy.Rate(1) rospy.sleep(1) a = 0.065 b = 0.040 c = 0.012 pattern_45 = [ [-0.10, 0.31, 0.015, 0, 0, 0, 1], #B 'c', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B #[0.0 ,0.31 ,0.15 , 0,0,0,1], #A90 [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], [0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795], 'o', [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], #[0.0 ,0.31 ,0.1 , 0,0,0,1], #A90 [-0.10, 0.401, 0.041, 0, 0, 0, 1], 'c', [-0.10, 0.401, 0.100, 0, 0, 0, 1], [0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795], [0, 0.5 - b, 0.037 + b, 0.3826834, 0, 0, 0.9238795], [0, 0.5 - c, 0.037 + c, 0.3826834, 0, 0, 0.9238795], 'o', 'c', [0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795], [-0.10, 0.401, 0.100, 0, 0, 0, 1], [-0.10, 0.401, 0.050, 0, 0, 0, 1], 'o', [-0.10, 0.401, 0.100, 0, 0, 0, 1], [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], [0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795], 'c', [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B [-0.10, 0.31, 0.015, 0, 0, 0, 1], #B 'o', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B ] while not rospy.is_shutdown(): for pose in pattern_45: if pose == 'c': self.gripper_close() rospy.sleep(1) elif pose == 'o': self.gripper_open() rospy.sleep(1) else: self.model_pose = pose self.reaching_pose() if rospy.is_shutdown(): return 0 def move_90(self): rate = rospy.Rate(1) rospy.sleep(1) a = 0.065 b = 0.040 c = 0.012 pattern_45 = [ [-0.10, 0.31, 0.015, 0, 0, 0, 1], #B 'c', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B [0.0, 0.300, 0.1, 0, 0, 0, 1], #A90 [0.0, 0.315, 0.0358, 0, 0, 0, 1], #A90 'o', [0.0, 0.315, 0.1, 0, 0, 0, 1], #A90 [-0.10, 0.40, 0.041, 0, 0, 0, 1], #negi 'c', [-0.10, 0.40, 0.1, 0, 0, 0, 1], [-0.001, 0.316, 0.1, 0, 0, 0, 1], #A90 [-0.001, 0.316, 0.053, 0, 0, 0, 1], #A90 'o', 'c', [0.0, 0.314, 0.1, 0, 0, 0, 1], #A90 [-0.10, 0.40, 0.1, 0, 0, 0, 1], [-0.10, 0.40, 0.05, 0, 0, 0, 1], #negi 'o', #[-0.10,0.40 ,0.1 , 0,0,0,1], [0.0, 0.300, 0.1, 0, 0, 0, 1], #A90 [0.0, 0.315, 0.036, 0, 0, 0, 1], #A90 'c', [0.0, 0.300, 0.1, 0, 0, 0, 1], #A90 [-0.10, 0.320, 0.1, 0, 0, 0, 1], #B [-0.10, 0.31, 0.0153, 0, 0, 0, 1], #B 'o', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B ] while not rospy.is_shutdown(): for pose in pattern_45: if pose == 'c': self.gripper_close() rospy.sleep(1) elif pose == 'o': self.gripper_open() rospy.sleep(1) else: self.model_pose = pose self.reaching_pose() if rospy.is_shutdown(): return 0
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) #ノードの初期化 reaching ていう名前 self.robot = moveit_commander.RobotCommander() # インスタンスの作成 self.model_pose = [0.0, 0.0, 0.0] #モデルの姿勢 self.obstacle_pose = [0.0, 0.0, 0.0] #障害物の位置 self.subsc_pose = [0.0, 0.0, 0.0] #購読した座標 self.subsc_orientation = [0.0, 0.0, 0.0, 1.0] #購読した四元数 self.set_forbidden() #禁止エリアの初期化 self.set_init_pose() #位置姿勢の初期化 self.before_pose = [0, 0, 0] #前回の場所 self.pcarray = 0 #pointcloud を np array にしたものを入れる self.executeFlag = False #unity コントローラ B ボタンと対応 self.before_executeFlag = False #前フレームの executeFlag self.waypoints = [] self.way_flag = False #unity コントローラの 中指のボタンに対応 self.before_wayflag = False #前フレームのway_flag self.state = { 'default': 0, 'hold': 1, 'way_standby': 2, 'plan_standby': 3 } # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 self.now_state = self.state['default'] #現在フレームの状態 def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface( "base_link") #PlanningSceneInterface のインスタンスの作成 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, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.0, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.0, -1.0) #座標に立方体を挿入 self.planning_scene.addCube("targetOBJ", 0.5, 0.0, 1.0, 2.0) self.planning_scene.removeCollisionObject("demo_cube") self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除 print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示 import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得 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 callback(self, data): #トピックにデータが追加されるたびに呼ばれる x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z self.model_pose = [x, y, z] #modelの姿勢を代入 x = data.pose[2].position.x y = data.pose[2].position.y z = data.pose[2].position.z self.obstacle_pose = [x, y, z] #障害物の姿勢を代入 def callbacksub(self, data): x = data.x y = data.y z = data.z self.subsc_pose = [x, y, z] #購読した座標を代入 def callbackunity(self, data): x = data.pose.position.x y = data.pose.position.y z = data.pose.position.z self.subsc_pose = [y, -x, z] #unity からの座標 座標変換も行っている x = data.pose.orientation.x y = data.pose.orientation.y z = data.pose.orientation.z w = data.pose.orientation.w self.subsc_orientation = [y, -x, z, w] #unity からの四元数 def rtabcallback(self, data): dtype_list = [(f.name, np.float32) for f in data.fields] cloud_arr = np.fromstring(data.data, dtype_list) self.pcarray = np.reshape(cloud_arr, (data.height, data.width)) def excallback(self, data): self.executeFlag = data.data def waycallback(self, data): self.way_flag = data.data def start_subscriber(self): #購読をスタート rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) # /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ #rospy.Subscriber("/UR/reaching/pose",Vector3,self.callbacksub) rospy.Subscriber("/unity/target", PoseStamped, self.callbackunity) #unityから配信しているトピックを購読 rospy.Subscriber('/rtabmap/cloud_map', PointCloud2, self.rtabcallback) #zed で slam したpointcloud を購読 rospy.Subscriber('/unity/execute', Bool, self.excallback) rospy.Subscriber('/unity/wayflag', Bool, self.waycallback) def change_state(self, target): #self.state={'default':0,'hold':1,'way_standby':2,'plan_standby':3} # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 set_flag = (self.before_pose != target.position ) # target の位置が変化している = トリガーが押された hold_flag = self.way_flag # hold button が押されている exe_flag = (self.before_executeFlag != self.executeFlag ) # executeFlag が前フレームと違う = execute button が押された print('set ' + str(set_flag)) print('hold ' + str(hold_flag)) print('exe ' + str(exe_flag)) if self.now_state == self.state['default']: if set_flag: self.now_state = self.state['plan_standby'] elif hold_flag: self.now_state = self.state['hold'] elif self.now_state == self.state['hold']: if not hold_flag: self.now_state = self.state['way_standby'] elif self.now_state == self.state['way_standby']: if exe_flag: self.now_state = self.state['default'] elif self.now_state == self.state['plan_standby']: if exe_flag: self.now_state = self.state['default'] def target(self): rate = rospy.Rate(1) #Rateクラスのインスタンス rateを1で rospy.sleep(1) #1秒スリープ useway = False while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、 self.planning_scene.removeCollisionObject("demo_cube") #オブジェクトの削除 self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除 self.planning_scene.addCube( "demo_cube", 0.06, self.model_pose[0], self.model_pose[1], self.model_pose[2]) #一辺が0.06の正方形をmodel_pose(座標)に追加 self.planning_scene.addBox( "obstacle", 0.540614, 0.083603, 0.54373, self.obstacle_pose[0], self.obstacle_pose[1], self.obstacle_pose[2]) #---の大きさの箱をobstacle_pose(座標)に追加 print self.model_pose #model_poseを表示 group = moveit_commander.MoveGroupCommander( "manipulator") #MoveGroupCommander クラスのインスタンス #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢 pose_target = geometry_msgs.msg.Pose( ) #geometry_msgs.pose のインスタンス ここにターゲット設定 pose_target.orientation.x = self.subsc_orientation[0] pose_target.orientation.y = self.subsc_orientation[1] pose_target.orientation.z = self.subsc_orientation[2] pose_target.orientation.w = self.subsc_orientation[ 3] #トピックから 四元数を代入 pose_target.position.x = self.subsc_pose[0] # pose_target.position.y = self.subsc_pose[1] # pose_target.position.z = self.subsc_pose[2] #トピックから 座標を代入 #self.planning_scene.removeCollisionObject("targetOBJ") #self.planning_scene.addCube("targetOBJ", 0.005,self.subsc_pose[0] ,self.subsc_pose[1] ,self.subsc_pose[2] ) self.way_flag = False #self.state={'default':0,'hold':1,'way_standby':2,'plan_standby':3} # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 self.change_state(pose_target) exe_flag = (self.before_executeFlag != self.executeFlag ) # executeFlag が前フレームと違う = execute button が押された if self.now_state == self.state['default']: if self.before_pose != pose_target.position: group.set_pose_target(pose_target) group.plan() if self.now_state == self.state['hold']: if self.before_pose != pose_target.position: self.waypoints.append(copy.deepcopy(pose_target)) if self.now_state == self.state['way_standby']: plan, fraction = group.compute_cartesian_path( self.waypoints, 0.01, 0.0) if exe_flag: group.execute(plan) self.waypoints = [] if self.now_state == self.state['plan_standby']: if exe_flag: group.go() self.before_executeFlag = self.executeFlag if self.before_pose != pose_target.position: #前フレームと座標が変化した if self.way_flag: #waypoint を使用したティーチング(グリップボタン押してる) self.waypoints.append(copy.deepcopy(pose_target)) useway = True else: group.set_pose_target(pose_target) #ターゲットをセット useway = False # group.set_pose_target(pose_target)#毎フレーム必要っぽい!!!!!!!!!!!!! if not useway and self.before_pose != pose_target.position and not self.before_wayflag: group.plan() if self.before_wayflag != self.way_flag and not self.way_flag: #前のフレームが true で false に変わったフレーム print(self.waypoints) plan, fraction = group.compute_cartesian_path( self.waypoints, 0.01, 0.0) self.before_pose = pose_target.position self.before_wayflag = self.way_flag if self.executeFlag != self.before_executeFlag: if useway: group.execute(plan) self.before_executeFlag = self.executeFlag self.waypoints = [] else: group.go() self.before_executeFlag = self.executeFlag rospy.sleep(1) #1秒スリープ
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 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 Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) #ノードの初期化 reaching ていう名前 self.robot = moveit_commander.RobotCommander() # インスタンスの作成 self.model_pose = [0.0, 0.0, 0.0] #モデルの姿勢 self.obstacle_pose = [0.0, 0.0, 0.0] #障害物の位置 self.subsc_pose = [0.0, 0.0, 0.0] #購読した座標 self.subsc_orientation = [0.0, 0.0, 0.0, 1.0] #購読した四元数 self.set_forbidden() #禁止エリアの初期化 self.set_init_pose() #位置姿勢の初期化 self.before_pose = [0, 0, 0] #前回の場所 self.pcarray = 0 #pointcloud を np array にしたものを入れる self.executeFlag = False #unity コントローラ B ボタンと対応 self.before_executeFlag = False #前フレームの executeFlag self.waypoints = [] self.way_flag = False #unity コントローラの 中指のボタンに対応 self.before_wayflag = False #前フレームのway_flag self.infirst_frame = True #最初のフレーム self.calc_way_flag = False self.frame = 0 # def set_ZEDbase(self): # listener = tf.TransformListener() # listener.waitForTransform('world','ee_link',rospy.Time(0),rospy.Duration(2.0)) # (self.trans,rot)=listener.lookupTransform('/world','/ee_link',rospy.Time(0)) # trans(x,y,z) rot (x,y,z,w) # print('trans = '+str(self.trans)) # br = tf.TransformBroadcaster() # br.sendTransform((self.trans[0],self.trans[1],self.trans[2]),(0.0,0.0,0.0,1.0),rospy.Time.now(),'base_forZED','world') def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface( "base_link") #PlanningSceneInterface のインスタンスの作成 self.planning_scene.removeCollisionObject("my_ground") self.planning_scene.addCube("my_ground", 2, 0, 0, -1.0) self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5) self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5) self.planning_scene.addCube("demo_cube", 0.06, 0, 0.3, 0) print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示 import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得 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 = [-1.26 , -0.64 , -2.44 , -0.66 , 1.56 , 0.007] # 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() #すべてのゴールをキャンセル self.state = { 'default': 0, 'hold': 1, 'way_standby': 2, 'plan_standby': 3 } # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 self.now_state = self.state['default'] #現在フレームの状態 # self.set_ZEDbase() def callback(self, data): #トピックにデータが追加されるたびに呼ばれる x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z self.model_pose = [x, y, z] #modelの姿勢を代入 x = data.pose[2].position.x y = data.pose[2].position.y z = data.pose[2].position.z self.obstacle_pose = [x, y, z] #障害物の姿勢を代入 def callbacksub(self, data): x = data.x y = data.y z = data.z self.subsc_pose = [x, y, z] #購読した座標を代入 def callbackunity(self, data): x = data.pose.position.x y = data.pose.position.y z = data.pose.position.z self.subsc_pose = [y, -x, z] #unity からの座標 座標変換も行っている x = data.pose.orientation.x y = data.pose.orientation.y z = data.pose.orientation.z w = data.pose.orientation.w self.subsc_orientation = [y, -x, z, w] #unity からの四元数 def rtabcallback(self, data): # dtype_list = [(f.name, np.float32) for f in data.fields] # cloud_arr = np.fromstring(data.data, dtype_list) # self.pcarray = np.reshape(cloud_arr, (data.height, data.width)) # print(self.pcarray) print('rtab subscribed ') def excallback(self, data): self.executeFlag = data.data #print(data.data) def waycallback(self, data): self.way_flag = data.data self.wayflagcalc() def wayflagcalc(self): if self.before_wayflag != self.way_flag: if self.calc_way_flag: self.calc_way_flag = False else: self.calc_way_flag = True self.before_wayflag = self.way_flag def start_subscriber(self): #購読をスタート rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) # /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ #rospy.Subscriber("/UR/reaching/pose",Vector3,self.callbacksub) rospy.Subscriber("/unity/target", PoseStamped, self.callbackunity) #unityから配信しているトピックを購読 #rospy.Subscriber('/rtabmap/cloud_map', PointCloud2, self.rtabcallback) #zed で slam したpointcloud を購読 rospy.Subscriber('/unity/execute', Bool, self.excallback) rospy.Subscriber('/unity/wayflag', Bool, self.waycallback) rospy.Subscriber('/pcl/near_points', PointCloud2, self.create_object) def change_state(self, target): #self.state = {'default':0,'hold':1,'way_standby':2,'plan_standby':3} # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 target が指定されたら plan_standby に以降 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 set_flag = (self.before_pose != target.position ) # target の位置が変化している = トリガーが押された #self.calc_way_flag # hold button が押されている exe_flag = (self.before_executeFlag != self.executeFlag ) # executeFlag が前フレームと違う = execute button が押された print('set ' + str(set_flag)) print('hold ' + str(self.calc_way_flag)) print('exe ' + str(exe_flag)) print('state ' + str(self.now_state)) if self.now_state == self.state['default']: if set_flag: self.now_state = self.state['plan_standby'] elif self.calc_way_flag: self.now_state = self.state['hold'] elif self.now_state == self.state['hold']: if not self.calc_way_flag: self.now_state = self.state['way_standby'] elif self.now_state == self.state['way_standby']: if set_flag: self.now_state = self.state['plan_standby'] elif exe_flag: self.now_state = self.state['default'] elif self.now_state == self.state['plan_standby']: if exe_flag: self.now_state = self.state['default'] elif self.calc_way_flag: self.now_state = self.state['hold'] def create_object(self, data): self.Points = [data[0:3] for data in pc2.read_points(data)] def target(self): rate = rospy.Rate(1) #Rateクラスのインスタンス rateを1で rospy.sleep(1) #1秒スリープ useway = False while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、 print self.model_pose #model_poseを表示 group = moveit_commander.MoveGroupCommander( "manipulator") #MoveGroupCommander クラスのインスタンス #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢 pose_target = geometry_msgs.msg.Pose( ) #geometry_msgs.pose のインスタンス ここにターゲット設定 pose_target.orientation.x = self.subsc_orientation[0] pose_target.orientation.y = self.subsc_orientation[1] pose_target.orientation.z = self.subsc_orientation[2] pose_target.orientation.w = self.subsc_orientation[ 3] #トピックから 四元数を代入 pose_target.position.x = self.subsc_pose[0] # pose_target.position.y = self.subsc_pose[1] # pose_target.position.z = self.subsc_pose[2] #トピックから 座標を代入 # br = tf.TransformBroadcaster() # br.sendTransform((self.trans[0],self.trans[1],self.trans[2]),(0.0,0.0,0.8509035,0.525322),rospy.Time.now(),'base_forZED','world') # #逐一ブロードキャストする if self.infirst_frame: self.before_pose = pose_target.position self.infirst_frame = False #self.planning_scene.removeCollisionObject("targetOBJ") #self.planning_scene.addCube("targetOBJ", 0.005,self.subsc_pose[0] ,self.subsc_pose[1] ,self.subsc_pose[2] ) self.way_flag = False #self.state={'default':0,'hold':1,'way_standby':2,'plan_standby':3} # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 exe_flag = (self.before_executeFlag != self.executeFlag ) # executeFlag が前フレームと違う = execute button が押された if self.now_state == self.state['default']: if self.before_pose != pose_target.position: group.set_pose_target(pose_target) print(' Set Target !!!\n') print(pose_target) if self.now_state == self.state['hold']: if self.before_pose != pose_target.position: self.waypoints.append(copy.deepcopy(pose_target)) print(' Append Target !!!') if self.now_state == self.state['way_standby']: plan, fraction = group.compute_cartesian_path( self.waypoints, 0.01, 0.0) if self.before_pose != pose_target.position: group.set_pose_target(pose_target) print(' Set Target !!!\n') print(pose_target) elif exe_flag: group.execute(plan) print(' Planning Execute !!!') print(self.waypoints) self.waypoints = [] if self.now_state == self.state['plan_standby']: group.set_pose_target(pose_target) group.plan() if self.before_pose != pose_target.position: group.set_pose_target(pose_target) print(' Set Target !!!\n') print(pose_target) elif exe_flag: group.go() print(' Planning Go !!!') self.change_state(pose_target) self.before_executeFlag = self.executeFlag self.before_pose = pose_target.position self.frame += 1 rospy.sleep(2) #2秒スリープ
if (rospy.Time.now() - t).to_sec() > 0.2: rospy.sleep(0.1) continue (item_translation, item_orientation) = tf_listener.lookupTransform( '/base_link', item_frame, t) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue gripper_goal.command.position = 0.15 gripper.send_goal(gripper_goal) gripper.wait_for_result(rospy.Duration(1.0)) print "item: " + str(item_translation) scene.addCube("item", 0.05, item_translation[0], item_translation[1], item_translation[2]) p.position.x = item_translation[0] - 0.01 - 0.06 p.position.y = item_translation[1] p.position.z = item_translation[2] + 0.04 + 0.14 p.orientation = Quaternion(*quaternion_from_euler(0, 1.2, 0)) arm.set_pose_target(p) arm.go(True) #os.system("rosservice call clear_octomap") gripper_goal.command.position = 0 gripper.send_goal(gripper_goal) gripper.wait_for_result(rospy.Duration(2.0)) scene.removeAttachedObject("item")
if __name__ == '__main__': rospy.init_node("hi") # Create move group interface for a fetch robot move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground1") planning_scene.removeCollisionObject("my_front_ground2") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground1", 0.5, 0.8, 0.25, 0.25) planning_scene.addCube("my_front_ground2", 0.5, 0.8, 0.25, 1.20) # This is the wrist link not the gripper itself gripper_frame = 'wrist_roll_link' # Position and rotation of two "wave end poses" gripper_poses = [ Pose(Point(0.042, 0.584, 1.426), Quaternion(0.173, -0.693, -0.242, 0.657)), Pose(Point(0.547, 0.30, 0.72), Quaternion(0.0, 0.0, 0.0, 1.0)) ] # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link'