def callback(states): scene = PlanningSceneInterface() robot = RobotCommander() arm = MoveGroupCommander("manipulator") arm.set_planner_id("RRTConnectkConfigDefault") arm.set_num_planning_attempts(20) arm.allow_replanning(True) pose = copy.deepcopy(states.pose[-1]) temp = tf.transformations.euler_from_quaternion( (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)) quaternion = tf.transformations.quaternion_from_euler( math.pi, temp[1], temp[2]) pose.position.z += 0.22 pose.orientation.x = quaternion[0] pose.orientation.y = quaternion[1] pose.orientation.z = quaternion[2] pose.orientation.w = quaternion[3] print pose arm.set_pose_target(pose) move_plan = arm.plan() i = 0 while (not move_plan.joint_trajectory.joint_names): move_plan = arm.plan() i += 1 if (i == 5): break state = arm.execute(move_plan)
class GenericDualArmClient(): _safe_kinematicsolver = "RRTConnectkConfigDefault" def __init__(self, *args, **kwargs): larm_name = args[0] rarm_name = args[1] # "arm_right" for Motoman SDA10F self.r_arm = MoveGroupCommander(rarm_name) self.r_arm.set_planner_id(self._safe_kinematicsolver) def move_rarm_shift_forward(self, axis, value): ''' Ref. http://docs.ros.org/indigo/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html#a22f9ec1477c3d61e728660850d50ce1f ''' self.r_arm.shift_pose_target(axis, value) self.r_arm.plan() self.r_arm.go() def move_rarm_fixedpose_forward(self): tpose = Pose() #TODO Currently the following position may work with SDA10F but not with NXO tpose.position.x = 0.599 tpose.position.y = -0.379 tpose.position.z = 1.11 self.r_arm.set_pose_target(tpose) self.r_arm.plan() self.r_arm.go()
def execute(self, userdata): mc = MoveGroupCommander("ext") mc.set_joint_value_target("ext_axis", userdata.position) plan = mc.plan() userdata.trajectory = plan.joint_trajectory if not plan.joint_trajectory.points: return 'aborted' return 'succeeded'
def execute(self, userdata): goal = userdata.goal mc = MoveGroupCommander("r" + str(userdata.robot) + "_arm") for joint in goal.__slots__: mc.set_joint_value_target("r" + str(userdata.robot) + "_joint_" + str(joint), getattr(goal, joint)) plan = mc.plan() userdata.trajectory = plan.joint_trajectory if not plan.joint_trajectory.points: return 'aborted' return 'succeeded'
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_speed_demo') # Initialize the move group for the right arm arm = MoveGroupCommander('manipulator') # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Allow some leeway in position(meters) and orientation (radians) arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.01) # Start the arm in the "home" pose stored in the SRDF file arm.set_named_target('home') arm.go() # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() # Start the arm in the "home" pose stored in the SRDF file arm.set_named_target('home') arm.go() # Get back the planned trajectory arm.set_joint_value_target(joint_positions) traj = arm.plan() # Scale the trajectory speed by a factor of 0.25 new_traj = scale_trajectory_speed(traj, 0.25) # Execute the new trajectory arm.execute(new_traj) rospy.sleep(1) arm.set_named_target('home') arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def execute(self, userdata): goal_r1 = MA1400JointState() goal_r2 = MA1400JointState() mc = MoveGroupCommander("arms") for joint in goal_r1.__slots__: mc.set_joint_value_target("r1_joint_" + str(joint), getattr(goal_r1, joint)) for joint in goal_r2.__slots__: mc.set_joint_value_target("r2_joint_" + str(joint), getattr(goal_r2, joint)) plan = mc.plan() userdata.trajectory = plan.joint_trajectory if not plan.joint_trajectory.points: return 'aborted' return 'succeeded'
def execute(self, userdata): group = resolve_planning_group(userdata.ik_link) if not group: return 'aborted' (goal, tip_frame) = transform_to_tip(group, userdata.ik_link, userdata.goal) mc = MoveGroupCommander(group) mc.set_pose_target(goal, tip_frame) plan = mc.plan() userdata.trajectory = plan.joint_trajectory if not plan.joint_trajectory.points: return 'aborted' return 'succeeded'
def execute(self, userdata): group = 'arms' mc = MoveGroupCommander(group) group_1 = resolve_planning_group(userdata.ik_link_1) (goal_1, tip_frame_1) = transform_to_tip(group_1, userdata.ik_link_1, userdata.goal_1) group_2 = resolve_planning_group(userdata.ik_link_2) (goal_2, tip_frame_2) = transform_to_tip(group_2, userdata.ik_link_2, userdata.goal_2) mc.set_pose_target(goal_1, tip_frame_1) mc.set_pose_target(goal_2, tip_frame_2) plan = mc.plan() userdata.trajectory = plan.joint_trajectory if not plan.joint_trajectory.points: return 'aborted' return 'succeeded'
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.02) right_arm.set_goal_orientation_tolerance(0.1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('rest') right_arm.go() # Move to the named "straight_forward" pose stored in the SRDF file right_arm.set_named_target('wave') right_arm.go() rospy.sleep(2) # Move back to the resting position at roughly 1/4 speed right_arm.set_named_target('rest') # Get back the planned trajectory traj = right_arm.plan() # Set the trajectory speed new_traj = create_tracking_trajectory(traj, 1.0) rospy.loginfo(new_traj) # Execute the new trajectory right_arm.execute(new_traj) rospy.sleep(1) # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def talker_by13(): #init moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_fk_demo') #cartesian = rospy.get_param('~cartesian', True) arm = MoveGroupCommander('manipulator') arm.set_pose_reference_frame('base_link') arm.allow_replanning(True) arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # arm.set_max_acceleration_scaling_factor(0.5) #arm.set_max_velocity_scaling_factor(0.5) end_effector_link = arm.get_end_effector_link() #arm.set_named_target('home') arm.set_named_target('up') arm.go() rospy.sleep(2) target_pose = PoseStamped() target_pose.header.frame_id = 'base_link' target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.86 target_pose.pose.position.y = 0.25 target_pose.pose.position.z = 0.02832 target_pose.pose.orientation.x = 0 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = 0 target_pose.pose.orientation.w = 1 #next: find workspace arm.set_start_state_to_current_state() arm.set_pose_target(target_pose, end_effector_link) traj = arm.plan() arm.execute(traj) #arm.shift_pose_target(2,-0.05,end_effector_link) #arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.02) right_arm.set_goal_orientation_tolerance(0.1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() # Move to the named "straight_forward" pose stored in the SRDF file right_arm.set_named_target('straight_forward') right_arm.go() rospy.sleep(2) # Move back to the resting position at roughly 1/4 speed right_arm.set_named_target('resting') # Get back the planned trajectory traj = right_arm.plan() # Scale the trajectory speed by a factor of 0.25 new_traj = scale_trajectory_speed(traj, 0.25) # Execute the new trajectory right_arm.execute(new_traj) rospy.sleep(1) # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class MoveCup(): def __init__(self): #basic initiatioon moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_tutorial') self.robot = moveit_commander.RobotCommander() ################ Collision Object self.scene = moveit_commander.PlanningSceneInterface() table = CollisionObject() primitive = SolidPrimitive() primitive.type = primitive.BOX box_pose = geometry_msgs.msg.PoseStamped() box_pose.header.stamp = rospy.Time.now() box_pose.header.frame_id = 'tablelol' box_pose.pose.position.x = 1.25 box_pose.pose.position.y = 0.0 box_pose.pose.position.z = -0.6 table.primitives.append(primitive) table.primitive_poses.append(box_pose) table.operation = table.ADD self.scene.add_box('table', box_pose, size=(.077, .073, .122)) #use joint_group parameter to change which arm it uses? self.joint_group = rospy.get_param('~arm', default="left_arm") self.group = MoveGroupCommander(self.joint_group) #self.group.set_planner_id("BKPIECEkConfigDefault") #this node will scale any tf pose requests to be at most max_reach from the base frame self.max_reach = rospy.get_param('~max_reach', default=1.1) #define a start pose that we can move to before stuff runs self.start_pose = PoseStamped() self.start_pose = self.get_start_pose() #remove this when working for realz self.display_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) self.rate = rospy.Rate(1) self.ik_srv = rospy.ServiceProxy( 'ExternalTools/left/PositionKinematicsNode/IKService', SolvePositionIK) self.limb = baxter_interface.Limb('left') self.rangesub = rospy.Subscriber('cup_center', geometry_msgs.msg.Point, self.rangecb) self.stop = False self.planning = False def rangecb(self, point): if self.planning and point.z == 0: rospy.loginfo('stop') self.stop = True self.move_start() self.rangesub.unregister() def callback(self, targetarray): #callback that moves in a constrained path to anything published to /target_poses ##First, scale the position to be withing self.max_reach #new_target = self.project_point(targetarray.data) # rospy.loginfo(self.stop) if not self.stop: self.planning = True new_target = self.project_point(targetarray) target = PoseStamped() target.header.stamp = rospy.Time.now() target.header.frame_id = 'base' target.pose.position = new_target #change orientation to be upright target.pose.orientation = self.start_pose.pose.orientation #clear group info and set it again self.group.clear_pose_targets() # self.group.set_path_constraints(self.get_constraint()) self.group.set_planning_time(10) # self.group.set_pose_target(target) ################### Try joint space planning jt_state = JointState() jt_state.header.stamp = rospy.Time.now() angles = self.limb.joint_angles() jt_state.name = list(angles.keys()) jt_state.position = list(angles.values()) jt_state.header.frame_id = 'base' result = self.ik_srv([target], [jt_state], 0) angles = {} i = 0 for name in result.joints[0].name: angles[name] = result.joints[0].position[i] i = i + 1 self.group.set_joint_value_target(angles) #plan and execute plan. If I find a way, I should add error checking her #currently, if the plan fails, it just doesn't move and waits for another pose to be published plan = self.group.plan() self.group.execute(plan) self.rate.sleep() return def scale_movegroup(self, vel=.9, acc=.9): #slows down baxters arm so we stop getting all those velocity limit errors self.group.set_max_velocity_scaling_factor(vel) self.group.set_max_acceleration_scaling_factor(acc) def unscale_movegroup(self): self.group.set_max_velocity_scaling_factor(1) self.group.set_max_acceleration_scaling_factor(1) def start_baxter_interface(self): #I copied this from an example but have no idea what it does self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate', UInt16, queue_size=10) self._left_arm = baxter_interface.limb.Limb("left") self._left_joint_names = self._left_arm.joint_names() print(self._left_arm.endpoint_pose()) self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() # set joint state publishing to 100Hz self._pub_rate.publish(100) return def get_start_pose(self, point=[.9, 0.2, 0], rpy=[0, math.pi / 2, 0]): #define a starting position for the move_start method new_p = PoseStamped() new_p.header.frame_id = self.robot.get_planning_frame() new_p.pose.position.x = point[0] new_p.pose.position.y = point[1] new_p.pose.position.z = point[2] p_orient = tf.transformations.quaternion_from_euler( rpy[0], rpy[1], rpy[2]) p_orient = Quaternion(*p_orient) new_p.pose.orientation = p_orient return (new_p) def project_point(self, multiarray): #scales an array and returns a point (see: Pose.position) to be within self.max_reach #convert points from sonar ring frame to shoulder frame x = multiarray.data[2] + sr[0] - lls[0] y = multiarray.data[0] + sr[1] - lls[1] z = (-1 * multiarray.data[1]) + sr[2] - lls[2] #scale point to a finite reach distance from the shoulder obj_dist = math.sqrt(x**2 + y**2 + z**2) scale_val = min(self.max_reach / obj_dist, .99) point_scaled = Point() #scale point and bring into the base frames point_scaled.x = scale_val * x + lls[0] point_scaled.y = scale_val * y + lls[1] point_scaled.z = scale_val * z + lls[2] return (point_scaled) def move_random(self): #moves baxter to a random position. used for testing randstate = PoseStamped() randstate = self.group.get_random_pose() self.group.clear_pose_targets() self.group.set_pose_target(randstate) self.group.set_planning_time(8) self.scale_movegroup() plan = self.group.plan() while len( plan.joint_trajectory.points) == 1 and not rospy.is_shutdown(): print('plan no work') plan = self.group.plan() self.group.execute(plan) self.rate.sleep() return def move_random_constrained(self): #move baxter to a random position with constrained path planning. also for testing self.scale_movegroup() randstate = PoseStamped() randstate = self.group.get_random_pose() self.group.clear_pose_targets() self.group.set_pose_target(randstate) self.group.set_path_constraints(self.get_constraint()) self.group.set_planning_time(100) self.scale_movegroup() constrained_plan = self.group.plan() self.group.execute(constrained_plan) self.unscale_movegroup() rospy.sleep(3) return def move_start(self): #move baxter to the self.start_pose pose self.group.clear_pose_targets() self.group.set_pose_target(self.start_pose) self.group.set_planning_time(10) print('moving to start') plan = self.group.plan() self.group.execute(plan) print('at start') self.rate.sleep() return def get_constraint(self, euler_orientation=[0, math.pi / 2, 0], tol=[3, 3, .5]): #method takes euler-angle inputs, this converts it to a quaternion q_orientation = tf.transformations.quaternion_from_euler( euler_orientation[0], euler_orientation[1], euler_orientation[2]) orientation_msg = Quaternion(q_orientation[0], q_orientation[1], q_orientation[2], q_orientation[3]) #defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down constraint = Constraints() constraint.name = 'upright_wrist' upright_orientation = OrientationConstraint() upright_orientation.link_name = self.group.get_end_effector_link() upright_orientation.orientation = orientation_msg upright_orientation.absolute_x_axis_tolerance = tol[0] upright_orientation.absolute_y_axis_tolerance = tol[1] upright_orientation.absolute_z_axis_tolerance = tol[2] upright_orientation.weight = 1.0 upright_orientation.header = self.start_pose.header constraint.orientation_constraints.append(upright_orientation) return (constraint)
'ur5_arm_shoulder_lift_joint', 'ur5_arm_elbow_joint', 'ur5_arm_wrist_1_joint', 'ur5_arm_wrist_2_joint', 'ur5_arm_wrist_3_joint'] msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.57, -0.1745, -2.79, -1.57, 0, 0], time_from_start=rospy.Duration(2))) client.send_goal(msg) client.wait_for_result() # open open_hand() # reach rospy.loginfo("reach") arm.set_pose_target([0.90, 0.16, 0.255, 0, 0, 0]) arm.plan() and arm.go() arm.plan() and arm.go() # approach rospy.loginfo("approach") arm.set_pose_target([1.13, 0.16, 0.255, 0, 0, 0]) arm.plan() and arm.go() # rotate for i in range(4): # close rospy.loginfo("close") close_hand() # rotate angles = arm.get_current_joint_values() import numpy start_angle = angles[5] print("current angles=", start_angle)
class MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # eel = len(self.right_arm.get_end_effector_link()) # print eel # Allow 5 seconds per planning attempt # self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) # self.m_error = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() ### Attach / Remove Object ### self.aro = None # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() ### GIVE SCENE TIME TO CATCH UP ### rospy.sleep(5) print "==================== Executing ===========================" blist = ['target','custom_2','custom_3', 'custom_table'] self.idx_list = [] for name in obj_id: print name if name not in blist: self.idx_list.append(obj_id.index(name)) ################################## TESTING AREA ##################################### # ga = self.grasp_attempt() # print ga # print obj_id success = False i=0 while success == False: idx = self.idx_list[i] print idx new_pose, ds = self.declutter_scene(idx) if ds == True: self.post_grasp(new_pose, obj_id[idx]) i+=1 ################# GRASPING ATTEMPTS ################### # ga = self.grasp_attempt() # print ga rospy.sleep(5) # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################# FUNCTIONS ################################################################################# def grasp_attempt(self): init_poses = [] grasp_poses = [] for axis in range(0,5): pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg') gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) gid, grasps = self.grasp_generator(grasp_poses) for i in range(0, len(gid)): self.gripper_pose_pub.publish(grasps[i]) rospy.sleep(0.1) success = False for pg in range(0,5): print pg plp = self.right_arm.plan(init_poses[pg].pose) print len(plp.joint_trajectory.points) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" continue self.right_arm.plan(init_poses[pg].pose) self.right_arm.go() if pg == 0: idx = gid.index('front') elif pg == 1: idx = gid.index('right') elif pg == 2: idx = gid.index('left') elif pg == 3: idx = gid.index('topx') else: idx = gid.index('topy') print ("idx = ",idx) for g in range(idx, len(gid)): ### TODO: fix the loop here, we dont want to check every single grasp after the index print g pl2 = self.right_arm.plan(grasps[g].pose) print len(pl2.joint_trajectory.points) if len(pl2.joint_trajectory.points) == 0: print "No Valid Grasp, continuing on next one" continue self.right_arm.plan(grasps[g].pose) self.right_arm.go() success = True break if success == True: break return success def declutter_scene(self,index): print obj_id[index] init_poses = [] grasp_poses = [] for axis in range(0,5): pg = self.grasp_pose(obj_pose[index], axis, 'pg') gp = self.grasp_pose(obj_pose[index], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) gid, grasps = self.grasp_generator(grasp_poses) for i in range(0, len(gid)): self.gripper_pose_pub.publish(grasps[i]) rospy.sleep(0.1) success = False for pg in range(0,5): print pg plp = self.right_arm.plan(init_poses[pg].pose) print len(plp.joint_trajectory.points) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" continue self.right_arm.plan(init_poses[pg].pose) self.right_arm.go() if pg == 0: idx = gid.index('front') elif pg == 1: idx = gid.index('right') elif pg == 2: idx = gid.index('left') elif pg == 3: idx = gid.index('topx') else: idx = gid.index('topy') print ("idx = ",idx) for g in range(idx, len(gid)): ### TODO: fix the loop here, we dont want to check every single grasp after the index print g pl2 = self.right_arm.plan(grasps[g].pose) print len(pl2.joint_trajectory.points) if len(pl2.joint_trajectory.points) == 0: print "No Valid Grasp, continuing on next one" continue self.right_arm.plan(grasps[g].pose) self.right_arm.go() new_pose = grasps[g] success = True break if success == True: break return (new_pose,success) def post_grasp(self,new_pose, obj_to_grasp): ######### GRASP OBJECT/ REMOVE FROM SCENE ######. self.close_gripper() self.aro = obj_to_grasp print self.aro # ### POST GRASP RETREAT ### # M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w]) # M1[0,3] = new_pose.pose.position.x # M1[1,3] = new_pose.pose.position.y # M1[2,3] = new_pose.pose.position.z # M2 = transformations.euler_matrix(0, 0, 0) # M2[0,3] = 0.0 # offset about x # M2[1,3] = 0.0 # about y # M2[2,3] = 0.3 # about z # T1 = np.dot(M1, M2) # npo = deepcopy(new_pose) # npo.pose.position.x = T1[0,3] # npo.pose.position.y = T1[1,3] # npo.pose.position.z = T1[2,3] # quat = transformations.quaternion_from_matrix(T1) # npo.pose.orientation.x = quat[0] # npo.pose.orientation.y = quat[1] # npo.pose.orientation.z = quat[2] # npo.pose.orientation.w = quat[3] # npo.header.frame_id = REFERENCE_FRAME # self.right_arm.plan(npo.pose) # self.right_arm.go() ### PLACE THE OBJECT place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.80 place_pose.pose.position.y = -0.33 place_pose.pose.position.z = 0.53 place_pose.pose.orientation.w = 1.0 def grasp_pose(self, target_pose, axis, stage): ############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE ######### if axis == 0: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 0) if stage == 'pg': M2[0,3] = -0.25 # offset about x elif stage == 'gp': M2[0,3] = -0.18 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.0 # about z elif axis == 1: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = -0.25 # about y elif stage == 'gp': M2[1,3] = -0.18 # about y M2[2,3] = 0.0 # about z elif axis ==2: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, -1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = 0.25 # about y elif stage == 'gp': M2[1,3] = 0.18 # about y M2[2,3] = 0.0 # about z elif axis ==3: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z else: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(1.57, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z T1 = np.dot(M1, M2) grasp_pose = deepcopy(target_pose) grasp_pose.pose.position.x = T1[0,3] grasp_pose.pose.position.y = T1[1,3] grasp_pose.pose.position.z = T1[2,3] quat = transformations.quaternion_from_matrix(T1) grasp_pose.pose.orientation.x = quat[0] grasp_pose.pose.orientation.y = quat[1] grasp_pose.pose.orientation.z = quat[2] grasp_pose.pose.orientation.w = quat[3] grasp_pose.header.frame_id = REFERENCE_FRAME return grasp_pose def grasp_generator(self, initial_poses): # A list to hold the grasps grasps = [] o = [] # Original Pose of the object (o) O=[] gid = [] i= 0 while i < len(initial_poses): o.append(initial_poses[i]) i+=1 G = transformations.euler_matrix(0, 0, 0) # Generate a grasps for along z axis (x and y directions) k = 0 while k <= 4: O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w])) O[k][0,3] = o[k].pose.position.x O[k][1,3] = o[k].pose.position.y O[k][2,3] = o[k].pose.position.z if k in range(0,3): for z in self.drange(-obj_size[obj_id.index('target')][2]/2, obj_size[obj_id.index('target')][2]/2, 0.02): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] +z quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) if k ==0: gid.append('front') elif k ==1: gid.append('right') elif k ==2: gid.append('left') elif k == 3: for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.01): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] +x grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) gid.append('topx') else: for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.01): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] +y grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) gid.append('topy') k+=1 # Return the list return (gid,grasps) def scene_generator(self): obj_pose =[] obj_id = [] obj_size = [] bl = ['ground_plane','pr2'] global obj_pose, obj_id , obj_size ops = PoseStamped() ops.header.frame_id = REFERENCE_FRAME for model_name in self.pwh.name: if model_name not in bl: obj_id.append(model_name) ops.pose = self.pwh.pose[self.pwh.name.index(model_name)] obj_pose.append(deepcopy(ops)) obj_size.append([0.05, 0.05, 0.15]) obj_id[obj_id.index('custom_1')] = 'target' obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10] obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05] obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03] ### REMOVE OBJECT FROM PREVIOUS RUNS ### for i in range(0, len(obj_id)): self.scene.remove_world_object(obj_id[i]) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index('target')]) next_call = time.time() while True: next_call = next_call+1 if self.aro is None: for i in range(0, len(obj_id)): ### CREATE THE SCENE ### self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i]) ### Make the target purple and obstacles orange ### self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0) self.setColor(obj_id[obj_id.index('custom_2')], 1, 0.623, 0, 1.0) self.setColor(obj_id[obj_id.index('custom_3')], 1, 0.623, 0, 1.0) self.setColor(obj_id[obj_id.index('custom_1_0')], 1, 0.623, 0, 1.0) # Send the colors to the planning scene self.sendColors() else: ###### ATTACH ITEM TO GRIPPER ###### touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link'] #print touch_links self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links) ### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ### self.scene.remove_world_object(obj_id[self.aro]) time.sleep(next_call - time.time()) def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def drange(self, start, stop, step): r = start while r < stop: yield r r += step def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p)
rospy.loginfo("Right Arm Pose Target 1:\n{}".format(pose_target_rarm_1)) pose_target_larm_1 = Pose() pose_target_larm_1.position.x = pose_target_rarm_1.position.x pose_target_larm_1.position.y = pose_target_rarm_1.position.y * -1.0 pose_target_larm_1.position.z = pose_target_rarm_1.position.z pose_target_larm_1.orientation.x = pose_target_rarm_1.orientation.x pose_target_larm_1.orientation.y = pose_target_rarm_1.orientation.y pose_target_larm_1.orientation.z = pose_target_rarm_1.orientation.z pose_target_larm_1.orientation.w = pose_target_rarm_1.orientation.w rospy.loginfo("Left Arm Pose Target 1:\n{}".format(pose_target_larm_1)) group.set_pose_target(pose_target_rarm_1, 'RARM_JOINT5_Link') group.set_pose_target(pose_target_larm_1, 'LARM_JOINT5_Link') group.plan() group.go() # Pose Target 2 pose_target_rarm_2 = Pose() pose_target_rarm_2.position.x = 0.3 pose_target_rarm_2.position.y = -0.3 pose_target_rarm_2.position.z = 0.5 pose_target_rarm_2.orientation.y = -1.0 rospy.loginfo("Right Arm Pose Target 2:\n{}".format(pose_target_rarm_2)) pose_target_larm_2 = Pose() pose_target_larm_2.position.x = pose_target_rarm_2.position.x pose_target_larm_2.position.y = pose_target_rarm_2.position.y * -1.0 pose_target_larm_2.position.z = pose_target_rarm_2.position.z
class SmartGrasper(object): """ This is the helper library to easily access the different functionalities of the simulated robot from python. """ __last_joint_state = None __current_model_name = "cricket_ball" __path_to_models = "/root/.gazebo/models/" def __init__(self): """ This constructor initialises the different necessary connections to the topics and services and resets the world to start in a good position. """ rospy.init_node("smart_grasper") self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState, self.__joint_state_cb, queue_size=1) rospy.wait_for_service("/gazebo/get_model_state", 10.0) rospy.wait_for_service("/gazebo/reset_world", 10.0) self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty) self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState) rospy.wait_for_service("/gazebo/pause_physics") self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty) rospy.wait_for_service("/gazebo/unpause_physics") self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) rospy.wait_for_service("/controller_manager/switch_controller") self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController) rospy.wait_for_service("/gazebo/set_model_configuration") self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration) rospy.wait_for_service("/gazebo/delete_model") self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel) rospy.wait_for_service("/gazebo/spawn_sdf_model") self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel) rospy.wait_for_service('/get_planning_scene', 10.0) self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True) self.arm_commander = MoveGroupCommander("arm") self.hand_commander = MoveGroupCommander("hand") self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory", FollowJointTrajectoryAction) self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False: rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.") raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.") if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False: rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.") raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.") self.reset_world() def reset_world(self): """ Resets the object poses in the world and the robot joint angles. """ self.__switch_ctrl.call(start_controllers=[], stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"], strictness=SwitchControllerRequest.BEST_EFFORT) self.__pause_physics.call() joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3', 'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3'] joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0] self.__set_model.call(model_name="smart_grasping_sandbox", urdf_param_name="robot_description", joint_names=joint_names, joint_positions=joint_positions) timer = Timer(0.0, self.__start_ctrl) timer.start() time.sleep(0.1) self.__unpause_physics.call() self.__reset_world.call() def get_object_pose(self): """ Gets the pose of the ball in the world frame. @return The pose of the ball. """ return self.__get_pose_srv.call(self.__current_model_name, "world").pose def get_tip_pose(self): """ Gets the current pose of the robot's tooltip in the world frame. @return the tip pose """ return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose def move_tip_absolute(self, target): """ Moves the tooltip to the absolute target in the world frame @param target is a geometry_msgs.msg.Pose @return True on success """ self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([target]) plan = self.arm_commander.plan() if not self.arm_commander.execute(plan): return False return True def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.): """ Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw. @return True on success """ transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw), PyKDL.Vector(-x, -y, -z)) tip_pose = self.get_tip_pose() tip_pose_kdl = posemath.fromMsg(tip_pose) final_pose = toMsg(tip_pose_kdl * transform) self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([final_pose]) plan = self.arm_commander.plan() if not self.arm_commander.execute(plan): return False return True def send_command(self, command, duration=0.2): """ Send a dictionnary of joint targets to the arm and hand directly. @param command: a dictionnary of joint names associated with a target: {"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0} @param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0 """ hand_goal = None arm_goal = None for joint, target in command.items(): if "H1" in joint: if not hand_goal: hand_goal = FollowJointTrajectoryGoal() point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(duration) hand_goal.trajectory.points.append(point) hand_goal.trajectory.joint_names.append(joint) hand_goal.trajectory.points[0].positions.append(target) else: if not arm_goal: arm_goal = FollowJointTrajectoryGoal() point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(duration) arm_goal.trajectory.points.append(point) arm_goal.trajectory.joint_names.append(joint) arm_goal.trajectory.points[0].positions.append(target) if arm_goal: self.__arm_traj_client.send_goal_and_wait(arm_goal) if hand_goal: self.__hand_traj_client.send_goal_and_wait(hand_goal) def get_current_joint_state(self): """ Gets the current state of the robot. @return joint positions, velocity and efforts as three dictionnaries """ joints_position = {n: p for n, p in zip(self.__last_joint_state.name, self.__last_joint_state.position)} joints_velocity = {n: v for n, v in zip(self.__last_joint_state.name, self.__last_joint_state.velocity)} joints_effort = {n: v for n, v in zip(self.__last_joint_state.name, self.__last_joint_state.effort)} return joints_position, joints_velocity, joints_effort def open_hand(self): """ Opens the hand. @return True on success """ self.hand_commander.set_named_target("open") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False return True def close_hand(self): """ Closes the hand. @return True on success """ self.hand_commander.set_named_target("close") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False return True def check_fingers_collisions(self, enable=True): """ Disables or enables the collisions check between the fingers and the objects / table @param enable: set to True to enable / False to disable @return True on success """ objects = ["cricket_ball__link", "drill__link", "cafe_table__link"] while self.__pub_planning_scene.get_num_connections() < 1: rospy.loginfo("waiting for someone to subscribe to the /planning_scene") rospy.sleep(0.1) request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = self.__get_planning_scene(request) acm = response.scene.allowed_collision_matrix for object_name in objects: if object_name not in acm.entry_names: # add object to allowed collision matrix acm.entry_names += [object_name] for row in range(len(acm.entry_values)): acm.entry_values[row].enabled += [False] new_row = deepcopy(acm.entry_values[0]) acm.entry_values += {new_row} for index_entry_values, entry_values in enumerate(acm.entry_values): if "H1_F" in acm.entry_names[index_entry_values]: for index_value, _ in enumerate(entry_values.enabled): if acm.entry_names[index_value] in objects: if enable: acm.entry_values[index_entry_values].enabled[index_value] = False else: acm.entry_values[index_entry_values].enabled[index_value] = True elif acm.entry_names[index_entry_values] in objects: for index_value, _ in enumerate(entry_values.enabled): if "H1_F" in acm.entry_names[index_value]: if enable: acm.entry_values[index_entry_values].enabled[index_value] = False else: acm.entry_values[index_entry_values].enabled[index_value] = True planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) self.__pub_planning_scene.publish(planning_scene_diff) rospy.sleep(1.0) return True def pick(self): """ Does its best to pick the ball. """ rospy.loginfo("Moving to Pregrasp") self.open_hand() time.sleep(0.1) ball_pose = self.get_object_pose() ball_pose.position.z += 0.5 #setting an absolute orientation (from the top) quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) ball_pose.orientation.x = quaternion[0] ball_pose.orientation.y = quaternion[1] ball_pose.orientation.z = quaternion[2] ball_pose.orientation.w = quaternion[3] self.move_tip_absolute(ball_pose) time.sleep(0.1) rospy.loginfo("Grasping") self.move_tip(y=-0.164) time.sleep(0.1) self.check_fingers_collisions(False) time.sleep(0.1) self.close_hand() time.sleep(0.1) rospy.loginfo("Lifting") for _ in range(5): self.move_tip(y=0.01) time.sleep(0.1) self.check_fingers_collisions(True) def swap_object(self, new_model_name): """ Replaces the current object with a new one.Replaces @new_model_name the name of the folder in which the object is (e.g. beer) """ try: self.__delete_model(self.__current_model_name) except: rospy.logwarn("Failed to delete: " + self.__current_model_name) try: sdf = None initial_pose = Pose() initial_pose.position.x = 0.15 initial_pose.position.z = 0.82 with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model: sdf = model.read() res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world") rospy.logerr( "RES: " + str(res) ) self.__current_model_name = new_model_name except: rospy.logwarn("Failed to delete: " + self.__current_model_name) def __compute_arm_target_for_ball(self): ball_pose = self.get_object_pose() # come at it from the top arm_target = ball_pose arm_target.position.z += 0.5 quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) arm_target.orientation.x = quaternion[0] arm_target.orientation.y = quaternion[1] arm_target.orientation.z = quaternion[2] arm_target.orientation.w = quaternion[3] return arm_target def __pre_grasp(self, arm_target): self.hand_commander.set_named_target("open") plan = self.hand_commander.plan() self.hand_commander.execute(plan, wait=True) for _ in range(10): self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([arm_target]) plan = self.arm_commander.plan() if self.arm_commander.execute(plan): return True def __grasp(self, arm_target): waypoints = [] waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose) arm_above_ball = deepcopy(arm_target) arm_above_ball.position.z -= 0.12 waypoints.append(arm_above_ball) self.arm_commander.set_start_state_to_current_state() (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0) print fraction if not self.arm_commander.execute(plan): return False self.hand_commander.set_named_target("close") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False self.hand_commander.attach_object("cricket_ball__link") def __lift(self, arm_target): waypoints = [] waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose) arm_above_ball = deepcopy(arm_target) arm_above_ball.position.z += 0.1 waypoints.append(arm_above_ball) self.arm_commander.set_start_state_to_current_state() (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0) print fraction if not self.arm_commander.execute(plan): return False def __start_ctrl(self): rospy.loginfo("STARTING CONTROLLERS") self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"], stop_controllers=[], strictness=1) def __joint_state_cb(self, msg): self.__last_joint_state = msg
class SrRobotCommander(object): """ Base class for hand and arm commanders """ def __init__(self, name): """ Initialize MoveGroupCommander object @param name - name of the MoveIt group """ self._name = name self._move_group_commander = MoveGroupCommander(name) self._robot_commander = RobotCommander() self._robot_name = self._robot_commander._r.get_robot_name() self.refresh_named_targets() self._warehouse_name_get_srv = rospy.ServiceProxy( "get_robot_state", GetState) self._planning_scene = PlanningSceneInterface() self._joint_states_lock = threading.Lock() self._joint_states_listener = \ rospy.Subscriber("joint_states", JointState, self._joint_states_callback, queue_size=1) self._joints_position = {} self._joints_velocity = {} self._joints_effort = {} self._joints_state = None self._clients = {} self.__plan = None self._controllers = {} rospy.wait_for_service('compute_ik') self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) controller_list_param = rospy.get_param("/move_group/controller_list") # create dictionary with name of controllers and corresponding joints self._controllers = { item["name"]: item["joints"] for item in controller_list_param } self._set_up_action_client(self._controllers) self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) threading.Thread(None, rospy.spin) def set_planner_id(self, planner_id): self._move_group_commander.set_planner_id(planner_id) def set_num_planning_attempts(self, num_planning_attempts): self._move_group_commander.set_num_planning_attempts( num_planning_attempts) def set_planning_time(self, seconds): self._move_group_commander.set_planning_time(seconds) def get_end_effector_pose_from_named_state(self, name): state = self._warehouse_name_get_srv(name, self._robot_name).state return self.get_end_effector_pose_from_state(state) def get_end_effector_pose_from_state(self, state): header = Header() fk_link_names = [self._move_group_commander.get_end_effector_link()] header.frame_id = self._move_group_commander.get_pose_reference_frame() response = self._forward_k(header, fk_link_names, state) return response.pose_stamped[0] def get_planning_frame(self): return self._move_group_commander.get_planning_frame() def set_pose_reference_frame(self, reference_frame): self._move_group_commander.set_pose_reference_frame(reference_frame) def get_group_name(self): return self._name def refresh_named_targets(self): self._srdf_names = self.__get_srdf_names() self._warehouse_names = self.__get_warehouse_names() def set_max_velocity_scaling_factor(self, value): self._move_group_commander.set_max_velocity_scaling_factor(value) def set_max_acceleration_scaling_factor(self, value): self._move_group_commander.set_max_acceleration_scaling_factor(value) def allow_looking(self, value): self._move_group_commander.allow_looking(value) def allow_replanning(self, value): self._move_group_commander.allow_replanning(value) def execute(self): """ Executes the last plan made. """ if self.check_plan_is_valid(): self._move_group_commander.execute(self.__plan) self.__plan = None else: rospy.logwarn("No plans were made, not executing anything.") def execute_plan(self, plan): if self.check_given_plan_is_valid(plan): self._move_group_commander.execute(plan) self.__plan = None else: rospy.logwarn("Plan is not valid, not executing anything.") def move_to_joint_value_target(self, joint_states, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self._move_group_commander.go(wait=wait) def plan_to_joint_value_target(self, joint_states, angle_degrees=False): """ Set target of the robot's links and plans. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param angle_degrees - are joint_states in degrees or not This is a blocking method. """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self.__plan = self._move_group_commander.plan() return self.__plan def check_plan_is_valid(self): """ Checks if current plan contains a valid trajectory """ return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0) def check_given_plan_is_valid(self, plan): """ Checks if given plan contains a valid trajectory """ return (plan is not None and len(plan.joint_trajectory.points) > 0) def get_robot_name(self): return self._robot_name def named_target_in_srdf(self, name): return name in self._srdf_names def set_named_target(self, name): if name in self._srdf_names: self._move_group_commander.set_named_target(name) elif (name in self._warehouse_names): response = self._warehouse_name_get_srv(name, self._robot_name) active_names = self._move_group_commander._g.get_active_joints() joints = response.state.joint_state.name positions = response.state.joint_state.position js = {} for n, this_name in enumerate(joints): if this_name in active_names: js[this_name] = positions[n] self._move_group_commander.set_joint_value_target(js) else: rospy.logerr("Unknown named state '%s'..." % name) return False return True def get_named_target_joint_values(self, name): output = dict() if (name in self._srdf_names): output = self._move_group_commander.\ _g.get_named_target_values(str(name)) elif (name in self._warehouse_names): js = self._warehouse_name_get_srv( name, self._robot_name).state.joint_state for x, n in enumerate(js.name): if n in self._move_group_commander._g.get_joints(): output[n] = js.position[x] else: rospy.logerr("No target named %s" % name) return None return output def get_end_effector_link(self): return self._move_group_commander.get_end_effector_link() def get_current_pose(self, reference_frame=None): """ Get the current pose of the end effector. @param reference_frame - The desired reference frame in which end effector pose should be returned. If none is passed, it will use the planning frame as reference. @return geometry_msgs.msg.Pose() - current pose of the end effector """ if reference_frame is not None: try: trans = self.tf_buffer.lookup_transform( reference_frame, self._move_group_commander.get_end_effector_link(), rospy.Time(0), rospy.Duration(5.0)) current_pose = geometry_msgs.msg.Pose() current_pose.position.x = trans.transform.translation.x current_pose.position.y = trans.transform.translation.y current_pose.position.z = trans.transform.translation.z current_pose.orientation.x = trans.transform.rotation.x current_pose.orientation.y = trans.transform.rotation.y current_pose.orientation.z = trans.transform.rotation.z current_pose.orientation.w = trans.transform.rotation.w return current_pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logwarn( "Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() + " in " + reference_frame + " reference frame") return None else: return self._move_group_commander.get_current_pose().pose def get_current_state(self): """ Get the current joint state of the group being used. @return a dictionary with the joint names as keys and current joint values """ joint_names = self._move_group_commander._g.get_active_joints() joint_values = self._move_group_commander._g.get_current_joint_values() return dict(zip(joint_names, joint_values)) def get_current_state_bounded(self): """ Get the current joint state of the group being used, enforcing that they are within each joint limits. @return a dictionary with the joint names as keys and current joint values """ current = self._move_group_commander._g.get_current_state_bounded() names = self._move_group_commander._g.get_active_joints() output = {n: current[n] for n in names if n in current} return output def get_robot_state_bounded(self): return self._move_group_commander._g.get_current_state_bounded() def move_to_named_target(self, name, wait=True): """ Set target of the robot's links and moves to it @param name - name of the target pose defined in SRDF @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self._move_group_commander.go(wait=wait) def plan_to_named_target(self, name): """ Set target of the robot's links and plans This is a blocking method. @param name - name of the target pose defined in SRDF """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self.__plan = self._move_group_commander.plan() def __get_warehouse_names(self): try: list_srv = rospy.ServiceProxy("list_robot_states", ListStates) return list_srv("", self._robot_name).states except rospy.ServiceException as exc: rospy.logwarn("Couldn't access warehouse: " + str(exc)) return list() def _reset_plan(self): self.__plan = None def _set_plan(self, plan): self.__plan = plan def __get_srdf_names(self): return self._move_group_commander._g.get_named_targets() def get_named_targets(self): """ Get the complete list of named targets, from SRDF as well as warehouse poses if available. @return list of strings containing names of targets. """ return self._srdf_names + self._warehouse_names def get_joints_position(self): """ Returns joints position @return - dictionary with joints positions """ with self._joint_states_lock: return self._joints_position def get_joints_velocity(self): """ Returns joints velocities @return - dictionary with joints velocities """ with self._joint_states_lock: return self._joints_velocity def _get_joints_effort(self): """ Returns joints effort @return - dictionary with joints efforts """ with self._joint_states_lock: return self._joints_effort def get_joints_state(self): """ Returns joints state @return - JointState message """ with self._joint_states_lock: return self._joints_state def run_joint_trajectory(self, joint_trajectory): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. """ plan = RobotTrajectory() plan.joint_trajectory = joint_trajectory self._move_group_commander.execute(plan) def make_named_trajectory(self, trajectory): """ Makes joint value trajectory from specified by named poses (either from SRDF or from warehouse) @param trajectory - list of waypoints, each waypoint is a dict with the following elements (n.b either name or joint_angles is required) - name -> the name of the way point - joint_angles -> a dict of joint names and angles - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent. """ current = self.get_current_state_bounded() joint_trajectory = JointTrajectory() joint_names = current.keys() joint_trajectory.joint_names = joint_names start = JointTrajectoryPoint() start.positions = current.values() start.time_from_start = rospy.Duration.from_sec(0.001) joint_trajectory.points.append(start) time_from_start = 0.0 for wp in trajectory: joint_positions = None if 'name' in wp.keys(): joint_positions = self.get_named_target_joint_values( wp['name']) elif 'joint_angles' in wp.keys(): joint_positions = copy.deepcopy(wp['joint_angles']) if 'degrees' in wp.keys() and wp['degrees']: for joint, angle in joint_positions.iteritems(): joint_positions[joint] = radians(angle) if joint_positions is None: rospy.logerr( "Invalid waypoint. Must contain valid name for named target or dict of joint angles." ) return None new_positions = {} for n in joint_names: new_positions[n] = joint_positions[ n] if n in joint_positions else current[n] trajectory_point = JointTrajectoryPoint() trajectory_point.positions = [ new_positions[n] for n in joint_names ] current = new_positions time_from_start += wp['interpolate_time'] trajectory_point.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(trajectory_point) if 'pause_time' in wp and wp['pause_time'] > 0: extra = JointTrajectoryPoint() extra.positions = trajectory_point.positions time_from_start += wp['pause_time'] extra.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(extra) return joint_trajectory def send_stop_trajectory_unsafe(self): """ Sends a trajectory of all active joints at their current position. This stops the robot. """ current = self.get_current_state_bounded() trajectory_point = JointTrajectoryPoint() trajectory_point.positions = current.values() trajectory_point.time_from_start = rospy.Duration.from_sec(0.1) trajectory = JointTrajectory() trajectory.points.append(trajectory_point) trajectory.joint_names = current.keys() self.run_joint_trajectory_unsafe(trajectory) def run_named_trajectory_unsafe(self, trajectory, wait=False): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory directly via contoller. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory_unsafe(joint_trajectory, wait) def run_named_trajectory(self, trajectory): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory via moveit. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory(joint_trajectory) def move_to_position_target(self, xyz, end_effector_link="", wait=True): """ Specify a target position for the end-effector and moves to it @param xyz - new position of end-effector @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_position_target(self, xyz, end_effector_link=""): """ Specify a target position for the end-effector and plans. This is a blocking method. @param xyz - new position of end-effector @param end_effector_link - name of the end effector link """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self.__plan = self._move_group_commander.plan() def move_to_pose_target(self, pose, end_effector_link="", wait=True): """ Specify a target pose for the end-effector and moves to it @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_pose_target(pose, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_pose_target(self, pose, end_effector_link="", alternative_method=False): """ Specify a target pose for the end-effector and plans. This is a blocking method. @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param alternative_method - use set_joint_value_target instead of set_pose_target """ self._move_group_commander.set_start_state_to_current_state() if alternative_method: self._move_group_commander.set_joint_value_target( pose, end_effector_link) else: self._move_group_commander.set_pose_target(pose, end_effector_link) self.__plan = self._move_group_commander.plan() return self.__plan def _joint_states_callback(self, joint_state): """ The callback function for the topic joint_states. It will store the received joint position, velocity and efforts information into dictionaries @param joint_state - the message containing the joints data. """ with self._joint_states_lock: self._joints_state = joint_state self._joints_position = { n: p for n, p in zip(joint_state.name, joint_state.position) } self._joints_velocity = { n: v for n, v in zip(joint_state.name, joint_state.velocity) } self._joints_effort = { n: v for n, v in zip(joint_state.name, joint_state.effort) } def _set_up_action_client(self, controller_list): """ Sets up an action client to communicate with the trajectory controller """ self._action_running = {} for controller_name in controller_list.keys(): self._action_running[controller_name] = False service_name = controller_name + "/follow_joint_trajectory" self._clients[controller_name] = SimpleActionClient( service_name, FollowJointTrajectoryAction) if self._clients[controller_name].wait_for_server( timeout=rospy.Duration(4)) is False: err_msg = 'Failed to connect to action server ({}) in 4 sec'.format( service_name) rospy.logwarn(err_msg) def move_to_joint_value_target_unsafe(self, joint_states, time=0.002, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param time - time in s (counting from now) for the robot to reach the target (it needs to be greater than 0.0 for it not to be rejected by the trajectory controller) @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ # self._update_default_trajectory() # self._set_targets_to_default_trajectory(joint_states) goals = {} joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [] point = JointTrajectoryPoint() point.positions = [] for x in joint_states_cpy.keys(): if x in controller_joints: goal.trajectory.joint_names.append(x) point.positions.append(joint_states_cpy[x]) point.time_from_start = rospy.Duration.from_sec(time) goal.trajectory.points = [point] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def action_is_running(self, controller=None): if controller is not None: return self._action_running[controller] for controller_running in self._action_running.values(): if controller_running: return True return False def _action_done_cb(self, controller, terminal_state, result): self._action_running[controller] = False def _call_action(self, goals): for client in self._clients: self._action_running[client] = True self._clients[client].send_goal( goals[client], lambda terminal_state, result: self._action_done_cb( client, terminal_state, result)) def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. @param wait - should method wait for movement end or not """ goals = {} for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory = copy.deepcopy(joint_trajectory) indices_of_joints_in_this_controller = [] for i, joint in enumerate(joint_trajectory.joint_names): if joint in controller_joints: indices_of_joints_in_this_controller.append(i) goal.trajectory.joint_names = [ joint_trajectory.joint_names[i] for i in indices_of_joints_in_this_controller ] for point in goal.trajectory.points: if point.positions: point.positions = [ point.positions[i] for i in indices_of_joints_in_this_controller ] if point.velocities: point.velocities = [ point.velocities[i] for i in indices_of_joints_in_this_controller ] if point.effort: point.effort = [ point.effort[i] for i in indices_of_joints_in_this_controller ] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def plan_to_waypoints_target(self, waypoints, reference_frame=None, eef_step=0.005, jump_threshold=0.0): """ Specify a set of waypoints for the end-effector and plans. This is a blocking method. @param reference_frame - the reference frame in which the waypoints are given @param waypoints - an array of poses of end-effector @param eef_step - configurations are computed for every eef_step meters @param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path """ old_frame = self._move_group_commander.get_pose_reference_frame() if reference_frame is not None: self.set_pose_reference_frame(reference_frame) (self.__plan, fraction) = self._move_group_commander.compute_cartesian_path( waypoints, eef_step, jump_threshold) self.set_pose_reference_frame(old_frame) def set_teach_mode(self, teach): """ Activates/deactivates the teach mode for the robot. Activation: stops the the trajectory controllers for the robot, and sets it to teach mode. Deactivation: stops the teach mode and starts trajectory controllers for the robot. Currently this method blocks for a few seconds when called on a hand, while the hand parameters are reloaded. @param teach - bool to activate or deactivate teach mode """ if teach: mode = RobotTeachModeRequest.TEACH_MODE else: mode = RobotTeachModeRequest.TRAJECTORY_MODE self.change_teach_mode(mode, self._name) def move_to_trajectory_start(self, trajectory, wait=True): """ Make and execute a plan from the current state to the first state in an pre-existing trajectory @param trajectory - moveit_msgs/JointTrajectory @param wait - Bool to specify if movement should block untill finished. """ if len(trajectory.points) <= 0: rospy.logerr("Trajectory has no points in it, can't reverse...") return None first_point = trajectory.points[0] end_state = dict(zip(trajectory.joint_names, first_point.positions)) self.move_to_joint_value_target(end_state, wait=wait) @staticmethod def change_teach_mode(mode, robot): teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode) req = RobotTeachModeRequest() req.teach_mode = mode req.robot = robot try: resp = teach_mode_client(req) if resp.result == RobotTeachModeResponse.ERROR: rospy.logerr("Failed to change robot %s to mode %d", robot, mode) else: rospy.loginfo("Changed robot %s to mode %d Result = %d", robot, mode, resp.result) except rospy.ServiceException: rospy.logerr("Failed to call service teach_mode") def get_ik(self, target_pose, avoid_collisions=False, joint_states=None): """ Computes the inverse kinematics for a given pose. It returns a JointState @param target_pose - A given pose of type PoseStamped @param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false """ service_request = PositionIKRequest() service_request.group_name = self._name service_request.ik_link_name = self._move_group_commander.get_end_effector_link( ) service_request.pose_stamped = target_pose service_request.timeout.secs = 0.5 service_request.avoid_collisions = avoid_collisions if joint_states is None: service_request.robot_state.joint_state = self.get_joints_state() else: service_request.robot_state.joint_state = joint_states try: resp = self._compute_ik(ik_request=service_request) # Check if error_code.val is SUCCESS=1 if resp.error_code.val != 1: if resp.error_code.val == -10: rospy.logerr("Unreachable point: Start state in collision") elif resp.error_code.val == -12: rospy.logerr("Unreachable point: Goal state in collision") elif resp.error_code.val == -31: rospy.logerr("Unreachable point: No IK solution") else: rospy.logerr("Unreachable point (error: %s)" % resp.error_code) return else: return resp.solution.joint_state except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(60) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "grasp" pose stored in the SRDF file right_arm.set_named_target('right_arm_up') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_OPEN) right_gripper.go() rospy.sleep(5) # Set the height of the table off the ground table_ground = 0.04 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] target_x = 0.135 #target_y = -0.32 target_y = -0.285290879999 # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.25 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = target_x target_pose.pose.position.y = target_y target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table blue and the boxes orange self.setColor(table_id, 0, 0, 0.8, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.18 place_pose.pose.position.y = 0 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 p = PoseStamped() p.header.frame_id = "up1_footprint" p.pose.position.x = 0.12792118579 p.pose.position.y = -0.285290879999 p.pose.position.z = 0.120301181892 p.pose.orientation.x = 0.0 p.pose.orientation.y = 0.0 p.pose.orientation.z = -0.706825181105 p.pose.orientation.w = 0.707388269167 right_gripper.set_pose_target(p.pose) # pick an object right_arm.allow_replanning(True) right_arm.allow_looking(True) right_arm.set_goal_tolerance(0.05) right_arm.set_planning_time(60) print "arm grasp" success = 0 attempt = 0 while not success: p_plan = right_arm.plan() attempt = attempt + 1 print "Planning attempt: " + str(attempt) if p_plan.joint_trajectory.points != []: success = 1 print "arm grasp" right_arm.execute(p_plan) rospy.sleep(5) right_gripper.set_joint_value_target(GRIPPER_GRASP) right_gripper.go() print "gripper closed" rospy.sleep(5) scene.attach_box(GRIPPER_FRAME, target_id) print "object attached" right_arm.set_named_target('right_arm_up') right_arm.go() print "arm up" rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class CalibrationMovements: def __init__(self, move_group_name, max_velocity_scaling, max_acceleration_scaling, angle_delta, translation_delta, move_group_namespace='/'): # self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic if not move_group_namespace.endswith('/'): move_group_namespace = move_group_namespace + '/' if move_group_namespace != '/': self.mgc = MoveGroupCommander( move_group_name, robot_description=move_group_namespace + 'robot_description', ns=move_group_namespace) else: self.mgc = MoveGroupCommander(move_group_name) self.mgc.set_planner_id("RRTConnectkConfigDefault" ) # TODO: this is only needed for the UR5 self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling) self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling) self.start_pose = self.mgc.get_current_pose() self.joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [ math.radians(350) ] # TODO: make param self.target_poses = [] self.current_pose_index = None self.current_plan = None self.fallback_joint_limits = [math.radians(90)] * 4 + [ math.radians(90) ] + [math.radians(180)] + [math.radians(350)] if len(self.mgc.get_active_joints()) == 6: self.fallback_joint_limits = self.fallback_joint_limits[1:] self.angle_delta = angle_delta self.translation_delta = translation_delta def set_and_check_starting_position(self): # sets the starting position to the current robot cartesian EE position and checks movement in all directions # TODO: make repeatable # - save the home position and each target position as joint position # - plan to each target position and back, save plan if not crazy # - return true if can plan to each target position without going crazy self.start_pose = self.mgc.get_current_pose() self.target_poses = self._compute_poses_around_state( self.start_pose, self.angle_delta, self.translation_delta) self.current_pose_index = -1 ret = self._check_target_poses(self.joint_limits) if ret: rospy.loginfo("Set current pose as home") return True else: rospy.logerr("Can't calibrate from this position!") self.start_pose = None self.target_poses = None return False def select_target_pose(self, i): number_of_target_poses = len(self.target_poses) if 0 <= i < number_of_target_poses: rospy.loginfo("Selected pose {} for next movement".format(i)) self.current_pose_index = i return True else: rospy.logerr( "Index {} is out of bounds: there are {} target poses".format( i, number_of_target_poses)) return False def plan_to_start_pose(self): # TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal rospy.loginfo("Planning to home pose") return self._plan_to_pose(self.start_pose) def plan_to_current_target_pose(self): # TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal i = self.current_pose_index rospy.loginfo("Planning to target pose {}".format(i)) return self._plan_to_pose(self.target_poses[i]) def execute_plan(self): if self.plan is None: rospy.logerr("No plan found!") return False if CalibrationMovements._is_crazy_plan(self.plan, self.fallback_joint_limits): rospy.logerr("Crazy plan found, not executing!") return False self.mgc.execute(self.plan) return True def _plan_to_pose(self, pose): self.mgc.set_start_state_to_current_state() self.mgc.set_pose_target(pose) ret = self.mgc.plan() if type(ret) is tuple: # noetic success, plan, planning_time, error_code = ret else: # melodic plan = ret if CalibrationMovements._is_crazy_plan(plan, self.fallback_joint_limits): rospy.logwarn("Planning failed") self.plan = None return False else: rospy.loginfo("Planning successful") self.plan = plan return True def _check_target_poses(self, joint_limits): if len(self.fallback_joint_limits) == 6: joint_limits = joint_limits[1:] for fp in self.target_poses: self.mgc.set_pose_target(fp) ret = self.mgc.plan() if type(ret) is tuple: # noetic success, plan, planning_time, error_code = ret else: # melodic plan = ret if len(plan.joint_trajectory.points ) == 0 or CalibrationMovements._is_crazy_plan( plan, joint_limits): return False return True @staticmethod def _compute_poses_around_state(start_pose, angle_delta, translation_delta): basis = np.eye(3) pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave final_rots = [] for qd in quaternion_deltas: final_rots.append(list(qd)) # TODO: accept a list of delta values pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta / 2) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta / 2)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave for qd in quaternion_deltas: final_rots.append(list(qd)) final_poses = [] for rot in final_rots: fp = deepcopy(start_pose) ori = fp.pose.orientation combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w], rot) fp.pose.orientation = Quaternion(*combined_rot) final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.x += translation_delta / 2 final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.x -= translation_delta / 2 final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.y += translation_delta final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.y -= translation_delta final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.z += translation_delta / 3 final_poses.append(fp) return final_poses @staticmethod def _rot_per_joint(plan, degrees=False): np_traj = np.array([p.positions for p in plan.joint_trajectory.points]) if len(np_traj) == 0: raise ValueError np_traj_max_per_joint = np_traj.max(axis=0) np_traj_min_per_joint = np_traj.min(axis=0) ret = abs(np_traj_max_per_joint - np_traj_min_per_joint) if degrees: ret = [math.degrees(j) for j in ret] return ret @staticmethod def _is_crazy_plan(plan, max_rotation_per_joint): abs_rot_per_joint = CalibrationMovements._rot_per_joint(plan) if (abs_rot_per_joint > max_rotation_per_joint).any(): return True else: return False
start_pose.position.x = -0.0988490064784 start_pose.position.y = 0.272349904278 start_pose.position.z = 1.18864499931 start_pose.orientation.x = 0.393751611087 start_pose.orientation.y = 0.918424640162 start_pose.orientation.z = -0.0150455838492 start_pose.orientation.w = 0.0350639347048 # start_pose.orientation.w = 0 # start_pose.orientation.x = 0 # start_pose.orientation.y = 1 # start_pose.orientation.z = 0 # start_pose.position.y = 0.0256415233819 # start_pose.position.z = 1.25871460368 # start_pose.position.x = 0.243500142238 right_arm.set_pose_target(start_pose) plan_start = right_arm.plan() print "============ Waiting while RVIZ displays plan_start..." rospy.sleep(5) right_arm.execute(plan_start) print "============ Waiting while RVIZ executes plan_start..." rospy.sleep(5) waypoints = [] waypoints.append(right_arm.get_current_pose().pose) gain = 0.2 points = 5 for i in xrange(points): wpose = geometry_msgs.msg.Pose() wpose.orientation.w = waypoints[i-1].orientation.w wpose.orientation.x = waypoints[i-1].orientation.x
joint_target_4, joint_target_5, joint_target_init ] joint_targets = [] for i in range(0, 361, 45): for j in range(-20, 21, 20): i_rads = i * math.pi / 180 j_rads = j * math.pi / 180 joint_target = rotate_joint(joint_target_init, i_rads, JOINT_INDEX_BASE) joint_target = rotate_joint(joint_target, j_rads, JOINT_INDEX_UPPER_ARM) joint_targets.append(joint_target) for joint_target in joint_targets: group.set_joint_value_target(joint_target) group.set_max_velocity_scaling_factor(0.1) plan = group.plan() group.go(wait=True) group.stop() if rospy.is_shutdown(): break rospy.sleep(1) roscpp_shutdown()
class Planner(object): move_group = None goals = None jspub = None namespace = None # These will eventually go to model objects robot_data = { 'group_name': 'right_arm_and_torso', 'eef_link': 'r_wrist_joint_link' } # Current state of the 'session' (right now, only one) current_scene = None status = None link_poses = None def __init__(self): rospy.init_node('moveit_web',disable_signals=True) self.jspub = rospy.Publisher('/update_joint_states',JointState) self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld) # Give time for subscribers to connect to the publisher rospy.sleep(1) self.goals = [] # HACK: Synthesize a valid initial joint configuration for PR2 initial_joint_state = JointState() initial_joint_state.name = ['r_elbow_flex_joint'] initial_joint_state.position = [-0.1] self.jspub.publish(initial_joint_state) # Create group we'll use all along this demo # self.move_group = MoveGroupCommander('right_arm_and_torso') self.move_group = MoveGroupCommander(self.robot_data['group_name']) self._move_group = self.move_group._g self.ps = PlanningSceneInterface() self.status = {'text':'ready to plan','ready':True} def get_scene(self): return self.current_scene def set_scene(self, scene): self.current_scene = scene psw = PlanningSceneWorld() for co_json in scene['objects']: # TODO: Fix orientation by using proper quaternions on the client pose = self._make_pose(co_json['pose']) # TODO: Decide what to do with STL vs. Collada. The client has a Collada # loader but the PlanningSceneInterface can only deal with STL. # TODO: Proper mapping between filenames and URLs # filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl'] filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl' co = self.ps.make_mesh(co_json['name'], pose, filename) psw.collision_objects.append(co) self.psw_pub.publish(psw) def get_link_poses(self): if self.link_poses is None: self.link_poses = self._move_group.get_link_poses_compressed() return self.link_poses # Create link back to socket.io namespace to allow emitting information def set_socket(self, namespace): self.namespace = namespace def emit(self, event, data=None): if self.namespace: self.namespace.emit(event, data) def emit_new_goal(self, pose): self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose']) def set_random_goal(self): goal_pose = self.move_group.get_random_pose() # goal_pose = self.move_group.get_random_pose('base_footprint') self.emit_new_goal(goal_pose) def _make_pose(self, json_pose): pose = PoseStamped() pose.header.frame_id = "odom_combined" pp = json_pose['position'] pose.pose.position.x = pp['x'] pose.pose.position.y = pp['y'] pose.pose.position.z = pp['z'] # TODO: Orientation is not working. See about # properly using Quaternions everywhere pp = json_pose['orientation'] pose.pose.orientation.x = pp['x'] pose.pose.orientation.y = pp['y'] pose.pose.orientation.z = pp['z'] pose.pose.orientation.w = pp['w'] return pose def plan_to_poses(self, poses): goal_pose = self._make_pose(poses[0]) self.move_group.set_pose_target(goal_pose) # self.move_group.set_pose_target(goal_pose,'base_footprint') self.emit('status',{'text':'Starting to plan'}) trajectory = self.move_group.plan() if trajectory is None or len(trajectory.joint_trajectory.joint_names) == 0: self.status = {'reachable':False,'text':'Ready to plan','ready':True} self.emit('status', self.status) else: self.status = {'reachable':True,'text':'Rendering trajectory'} self.emit('status', self.status) self.publish_trajectory(trajectory) def publish_goal_position(self, trajectory): self.publish_position(self, trajectory, -1) def publish_position(self, trajectory, step): jsmsg = JointState() jsmsg.name = trajectory.joint_trajectory.joint_names jsmsg.position = trajectory.joint_trajectory.points[step].positions self.jspub.publish(jsmsg) def publish_trajectory(self, trajectory): cur_time = 0.0 acceleration = 4.0 for i in range(len(trajectory.joint_trajectory.points)): point = trajectory.joint_trajectory.points[i] gevent.sleep((point.time_from_start - cur_time)/acceleration) cur_time = point.time_from_start # self.publish_position(trajectory, i) # TODO: Only say "True" to update state on the last step of the trajectory new_poses = self._move_group.update_robot_state(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].positions, True) self.link_poses = new_poses self.emit('link_poses', new_poses) self.status = {'text':'Ready to plan','ready':True} self.emit('status', self.status)
class MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) self.left_arm = MoveGroupCommander('left_arm') # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.left_gripper = MoveGroupCommander('left_gripper') # eel = len(self.right_arm.get_end_effector_link()) # print eel # Allow 5 seconds per planning attempt # self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ### Attach / Remove Object Flag ### self.aro = None # Run and keep in the BG the scene generator with ctrl^c kill ### timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() ## Give some time to ensure the thread starts!! ## rospy.sleep(5) ### GENERATE THE BLACKLIST AND REMOVE ATTACHED OBJECTS FROM PREVIOUS RUNS ### self.idx_list = self.bl() ### GIVE SCENE TIME TO CATCH UP ### rospy.sleep(5) ################################## GRASP EXECUTION ##################################### print "==================== Executing ===========================" start_time = time.time() ### PERSONAL REMINDER!!! WHAT IS WHAT!!! ### # print obj_id[obj_id.index('target')] # print obj_id.index('target') ### MOVE LEFT ARM OUT OF THE WAY ### self.lasp() success = False while success == False and len(self.idx_list)>0: success, pgr_target = self.grasp_attempt() print ("GA Returns:", success) if success is not False: self.flag = 0 # To let the planning scene know when to remove the object self.post_grasp(pgr_target, obj_id.index('target'),'true') self.place_object(obj_id.index('target')) break else: idx = self.idx_list[0] ds, pgr_col_obj = self.declutter_scene(idx) print ("DS Returns:", ds) if ds == True: self.flag = 0 # To let the planning scene know when to remove the object self.post_grasp(pgr_col_obj, obj_id.index(obj_id[idx]),'true') self.place_object(obj_id.index(obj_id[idx])) self.idx_list.pop(0) print "==================== THE END! ======================" print("--- %s seconds ---" % (time.time() - start_time)) rospy.sleep(5) # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################# FUNCTIONS ################################################################################# def grasp_attempt(self): # start_time = time.time() retreat = None init_poses = [] grasp_poses = [] for axis in range(0,6): # while obj_id[obj_id.index('target')] is not 'target': # print '!!!!!' # rospy.sleep(0.05) pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg') gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) pre_grasps = self.grasp_generator(init_poses) grasps = self.grasp_generator(grasp_poses) for grasp in grasps: self.gripper_pose_pub.publish(grasp) rospy.sleep(0.05) success = False i = 1 for pg, gr in izip(pre_grasps, grasps): self.gripper_pose_pub.publish(gr) print ("G Attempt: ", i) plp = self.right_arm.plan(pg.pose) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" i+=1 continue i+=1 self.right_arm.plan(pg.pose) self.right_arm.go(wait=True) rospy.sleep(5) plg = self.right_arm.plan(gr.pose) if len(plg.joint_trajectory.points) >= 10: self.right_arm.go() success = True retreat = gr print "Grasping" break # print("--- %s seconds ---" % (time.time() - start_time)) return success , retreat def declutter_scene(self,index): retreat = None init_poses = [] grasp_poses = [] for axis in range(0,6): pg = self.grasp_pose(obj_pose[index], axis, 'pg') gp = self.grasp_pose(obj_pose[index], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) pre_grasps = self.grasp_generator(init_poses) grasps = self.grasp_generator(grasp_poses) for grasp in grasps: self.gripper_pose_pub.publish(grasp) rospy.sleep(0.05) success = False i= 1 for pg, gr in izip(pre_grasps, grasps): plp = self.right_arm.plan(pg.pose) print (" DS Attempt: ", i) self.gripper_pose_pub.publish(gr) self.right_arm.plan(pg.pose) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" i+=1 continue i+=1 self.right_arm.plan(pg.pose) self.right_arm.go() rospy.sleep(5) plg = self.right_arm.plan(gr.pose) if len(plg.joint_trajectory.points) >= 10: self.right_arm.go() print "Grasping" success = True retreat = gr break return success, retreat def place_object(self, obj_idx): self.aro = obj_idx ### GENERATE PLACE POSES ### places = self.place_generator() ### TRY THESE POSES ### i = 1 for place in places: print (" Place Attempt: ", i) plpl = self.right_arm.plan(place.pose) print len(plpl.joint_trajectory.points) if len(plpl.joint_trajectory.points) == 0: i+=1 continue self.right_arm.plan(plpl) self.right_arm.go(wait=True) ### INFORM SCENE ### # self.open_gripper() # self.aro = None ### RETURN HAND TO STARTING POSITION ### self.post_grasp(place,obj_idx, 'false') self.rasp() break def post_grasp(self,new_pose, obj_idx, fl): ######### GRASP OBJECT/ REMOVE FROM SCENE ######. if fl == 'true': self.close_gripper() self.aro = obj_idx else: self.open_gripper() self.aro = None rospy.sleep(2) ### POST GRASP RETREAT ### M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w]) M1[0,3] = new_pose.pose.position.x M1[1,3] = new_pose.pose.position.y M1[2,3] = new_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T1 = np.dot(M2, M1) npo = deepcopy(new_pose) npo.pose.position.x = T1[0,3] npo.pose.position.y = T1[1,3] npo.pose.position.z = T1[2,3] quat = transformations.quaternion_from_matrix(T1) npo.pose.orientation.x = quat[0] npo.pose.orientation.y = quat[1] npo.pose.orientation.z = quat[2] npo.pose.orientation.w = quat[3] npo.header.frame_id = REFERENCE_FRAME self.right_arm.plan(npo.pose) self.right_arm.go(wait=True) def place_generator(self): place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.57 place_pose.pose.position.y = 0.16 place_pose.pose.position.z = 0.56 place_pose.pose.orientation.w = 1.0 P = transformations.quaternion_matrix([place_pose.pose.orientation.x, place_pose.pose.orientation.y, place_pose.pose.orientation.z, place_pose.pose.orientation.w]) P[0,3] = place_pose.pose.position.x P[1,3] = place_pose.pose.position.y P[2,3] = place_pose.pose.position.z places =[] yaw_angles = [0, 1,57, -1,57 , 3,14] x_vals = [0, 0.05 ,0.1 , 0.15] z_vals = [0.05 ,0.1 , 0.15] for y in yaw_angles: G = transformations.euler_matrix(0, 0, y) G[0,3] = 0.0 # offset about x G[1,3] = 0.0 # about y G[2,3] = 0.0 # about z for z in z_vals: for x in x_vals: TM = np.dot(P, G) pl = deepcopy(place_pose) pl.pose.position.x = TM[0,3] +x pl.pose.position.y = TM[1,3] pl.pose.position.z = TM[2,3] +z quat = transformations.quaternion_from_matrix(TM) pl.pose.orientation.x = quat[0] pl.pose.orientation.y = quat[1] pl.pose.orientation.z = quat[2] pl.pose.orientation.w = quat[3] pl.header.frame_id = REFERENCE_FRAME places.append(deepcopy(pl)) return places def grasp_pose(self, target_pose, axis, stage): ############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE ######### if axis == 0: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 0) if stage == 'pg': M2[0,3] = -0.25 # offset about x elif stage == 'gp': M2[0,3] = -0.18 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.0 # about z elif axis == 1: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = -0.25 # about y elif stage == 'gp': M2[1,3] = -0.18 # about y M2[2,3] = 0.0 # about z elif axis == 2: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, -1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = 0.25 # about y elif stage == 'gp': M2[1,3] = 0.18 # about y M2[2,3] = 0.0 # about z elif axis == 3: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 3.14) if stage == 'pg': M2[0,3] = 0.25 # offset about x elif stage == 'gp': M2[0,3] = 0.18 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.0 # about z elif axis == 4: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z else: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(1.57, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z T1 = np.dot(M1, M2) grasp_pose = deepcopy(target_pose) grasp_pose.pose.position.x = T1[0,3] grasp_pose.pose.position.y = T1[1,3] grasp_pose.pose.position.z = T1[2,3] quat = transformations.quaternion_from_matrix(T1) grasp_pose.pose.orientation.x = quat[0] grasp_pose.pose.orientation.y = quat[1] grasp_pose.pose.orientation.z = quat[2] grasp_pose.pose.orientation.w = quat[3] grasp_pose.header.frame_id = REFERENCE_FRAME return grasp_pose def grasp_generator(self, initial_poses): # A list to hold the grasps grasps = [] o = [] # Original Pose of the object (o) O=[] i= 0 while i < len(initial_poses): o.append(initial_poses[i]) i+=1 G = transformations.euler_matrix(0, 0, 0) # Generate a grasps for along z axis (x and y directions) k = 0 while k <= 5: O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w])) O[k][0,3] = o[k].pose.position.x O[k][1,3] = o[k].pose.position.y O[k][2,3] = o[k].pose.position.z if k in range(0,4): for z in self.drange(0.005-obj_size[obj_id.index('target')][2]/2,-0.005 + obj_size[obj_id.index('target')][2]/2, 0.02): ### TODO: USE EACH OBJECTS SIZE NOT ONLY THE TARGETS ### # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] +z quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) elif k == 4: for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] +x grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) else: for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] +y grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) k+=1 print len(grasps) # Return the list return grasps def scene_generator(self): while True: # print "happening" obj_pose =[] obj_id = [] obj_size = [] bl = ['ground_plane','pr2'] global obj_pose, obj_id , obj_size ops = PoseStamped() ops.header.frame_id = REFERENCE_FRAME for model_name in self.pwh.name: if model_name not in bl: obj_id.append(model_name) ops.pose = self.pwh.pose[self.pwh.name.index(model_name)] obj_pose.append(deepcopy(ops)) obj_size.append([0.05, 0.05, 0.15]) # obj_id[obj_id.index('custom_1')] = 'target' obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10] obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05] obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03] if self.aro is None: for i in range(0, len(obj_id)): ### CREATE THE SCENE ### self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i]) self.setColor(obj_id[i], 1, 0.623, 0, 1.0) ### Make the target purple and table green ### self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0) self.setColor(obj_id[obj_id.index('custom_table')], 0.3, 1, 0.3, 1.0) self.scene.remove_attached_object(GRIPPER_FRAME) # Send the colors to the planning scene self.sendColors() else: if self.flag == 0: touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link', 'r_wrist_roll_link', 'r_upper_arm_link'] #print touch_links self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links) ### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ### self.scene.remove_world_object(obj_id[self.aro]) self.flag +=1 time.sleep(0.5) def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def bl(self): blist = ['target','custom_2','custom_3', 'custom_table'] self.blist = [] for name in obj_id: if name not in blist: self.blist.append(obj_id.index(name)) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index(name)]) self.scene.remove_attached_object(GRIPPER_FRAME, 'target') return self.blist def drange(self, start, stop, step): r = start while r < stop: yield r r += step def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.044, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0899, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p) def lasp(self): sp = PoseStamped() sp.header.frame_id = REFERENCE_FRAME sp.pose.position.x = 0.3665 sp.pose.position.y = 0.74094 sp.pose.position.z = 1.1449 sp.pose.orientation.x = 0.80503 sp.pose.orientation.y = -0.18319 sp.pose.orientation.z = 0.31988 sp.pose.orientation.w = 0.46481 self.left_arm.plan(sp) self.left_arm.go(wait=True) def rasp(self): sp = PoseStamped() sp.header.frame_id = REFERENCE_FRAME sp.pose.position.x = 0.39571 sp.pose.position.y = -0.40201 sp.pose.position.z = 1.1128 sp.pose.orientation.x =0.00044829 sp.pose.orientation.y = 0.57956 sp.pose.orientation.z = 9.4878e-05 sp.pose.orientation.w = 0.81493 self.right_arm.plan(sp) self.right_arm.go(wait=True)
mg = MoveGroupCommander('right_arm_and_torso') p = mg.get_current_pose() print "Start pose:" print p p.pose.position.x += 0.3 #ps = PlanningSceneInterface() #psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld) #rospy.sleep(1) #co = ps.make_sphere("test_co", p, 0.02) #psw = PlanningSceneWorld() #psw.collision_objects.append(co) #psw_pub.publish(psw) # ps.remove_world_object("test_sphere") # ps.add_sphere("test_sphere", p, 0.1) # rospy.sleep(1) # ps.add_sphere("test_sphere", p, 0.1) #p.pose.position.x += 0.3 print "End pose:" print p mg.set_pose_target(mg.get_random_pose()) traj = mg.plan() print traj
print ">>>>> Printing robot state" print robot.get_current_state() print ">>>>> Printing robot pose" print group.get_current_pose() ## Planning to a Pose goal print ">>>>> Generating plan" pose_target = geometry_msgs.msg.Pose() #pose_target.orientation.w = 1.0 pose_target.position.x = 0.5 #0.4 pose_target.position.y = 0.2 #0.2 pose_target.position.z = 0.2 #0.2 group.set_pose_target(pose_target) plan = group.plan() #print "============ Waiting while RVIZ displays plan..." rospy.sleep(1) print ">>>>> Go for plan" group.go(wait=True) ## Adding/Removing Objects and Attaching/Detaching Objects collision_object = moveit_msgs.msg.CollisionObject() moveit_commander.roscpp_shutdown() rospy.spin() roscpp_shutdown()
class PlannerAnnotationParser(AnnotationParserBase): """ Parses the annotations files that contains the benchmarking information. """ def __init__(self, path_to_annotation, path_to_data): super(PlannerAnnotationParser, self).__init__(path_to_annotation, path_to_data) self.parse() self._load_scene() self._init_planning() self.benchmark() def check_results(self, results): """ Returns the results from the planner, checking them against any eventual validation data (no validation data in our case). """ return self.planner_data def _load_scene(self): """ Loads the proper scene for the planner. It can be either a python static scene or bag containing an occupancy map. """ scene = self._annotations["scene"] for element in scene: if element["type"] == "launch": self.play_launch(element["name"]) elif element["type"] == "python": self.load_python(element["name"]) elif element["type"] == "bag": self.play_bag(element["name"]) for _ in range(150): rospy.sleep(0.3) # wait for the scene to be spawned properly rospy.sleep(0.5) def _init_planning(self): """ Initialises the needed connections for the planning. """ self.group_id = self._annotations["group_id"] self.planners = self._annotations["planners"] self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.group = MoveGroupCommander(self.group_id) self._marker_pub = rospy.Publisher('/visualization_marker_array', MarkerArray, queue_size=10, latch=True) self._planning_time_sub = rospy.Subscriber( '/move_group/result', MoveGroupActionResult, self._check_computation_time) rospy.sleep(1) self.group.set_num_planning_attempts( self._annotations["planning_attempts"]) self.group.set_goal_tolerance(self._annotations["goal_tolerance"]) self.group.set_planning_time(self._annotations["planning_time"]) self.group.allow_replanning(self._annotations["allow_replanning"]) self._comp_time = [] self.planner_data = [] def benchmark(self): for test_id, test in enumerate(self._annotations["tests"]): marker_position_1 = test["start_xyz"] marker_position_2 = test["goal_xyz"] self._add_markers(marker_position_1, "Start test \n sequence", marker_position_2, "Goal") # Start planning in a given joint position joints = test["start_joints"] current = RobotState() current.joint_state.name = self.robot.get_current_state( ).joint_state.name current_joints = list( self.robot.get_current_state().joint_state.position) current_joints[0:6] = joints current.joint_state.position = current_joints self.group.set_start_state(current) joints = test["goal_joints"] for planner in self.planners: if planner == "stomp": planner = "STOMP" elif planner == "sbpl": planner = "AnytimeD*" self.planner_id = planner self.group.set_planner_id(planner) self._plan_joints( joints, self._annotations["name"] + "-test_" + str(test_id)) return self.planner_data def _add_markers(self, point, text1, point_2, text2): # add marker for start and goal pose of EE marker_array = MarkerArray() marker_1 = Marker() marker_1.header.frame_id = "world" marker_1.header.stamp = rospy.Time.now() marker_1.type = Marker.SPHERE marker_1.scale.x = 0.04 marker_1.scale.y = 0.04 marker_1.scale.z = 0.04 marker_1.lifetime = rospy.Duration() marker_2 = deepcopy(marker_1) marker_1.color.g = 0.5 marker_1.color.a = 1.0 marker_1.id = 0 marker_1.pose.position.x = point[0] marker_1.pose.position.y = point[1] marker_1.pose.position.z = point[2] marker_2.color.r = 0.5 marker_2.color.a = 1.0 marker_2.id = 1 marker_2.pose.position.x = point_2[0] marker_2.pose.position.y = point_2[1] marker_2.pose.position.z = point_2[2] marker_3 = Marker() marker_3.header.frame_id = "world" marker_3.header.stamp = rospy.Time.now() marker_3.type = Marker.TEXT_VIEW_FACING marker_3.scale.z = 0.10 marker_3.lifetime = rospy.Duration() marker_4 = deepcopy(marker_3) marker_3.text = text1 marker_3.id = 2 marker_3.color.g = 0.5 marker_3.color.a = 1.0 marker_3.pose.position.x = point[0] marker_3.pose.position.y = point[1] marker_3.pose.position.z = point[2] + 0.15 marker_4.text = text2 marker_4.id = 3 marker_4.color.r = 0.5 marker_4.color.a = 1.0 marker_4.pose.position.x = point_2[0] marker_4.pose.position.y = point_2[1] marker_4.pose.position.z = point_2[2] + 0.15 marker_array.markers = [marker_1, marker_2, marker_3, marker_4] self._marker_pub.publish(marker_array) rospy.sleep(1) def _plan_joints(self, joints, test_name): # plan to joint target and determine success self.group.clear_pose_targets() group_variable_values = self.group.get_current_joint_values() group_variable_values[0:6] = joints[0:6] self.group.set_joint_value_target(group_variable_values) plan = self.group.plan() plan_time = "N/A" total_joint_rotation = "N/A" comp_time = "N/A" plan_success = self._check_plan_success(plan) if plan_success: plan_time = self._check_plan_time(plan) total_joint_rotation = self._check_plan_total_rotation(plan) while not self._comp_time: rospy.sleep(0.5) comp_time = self._comp_time.pop(0) self.planner_data.append([ self.planner_id, test_name, str(plan_success), plan_time, total_joint_rotation, comp_time ]) @staticmethod def _check_plan_success(plan): if len(plan.joint_trajectory.points) > 0: return True else: return False @staticmethod def _check_plan_time(plan): # find duration of successful plan number_of_points = len(plan.joint_trajectory.points) time = plan.joint_trajectory.points[number_of_points - 1].time_from_start.to_sec() return time @staticmethod def _check_plan_total_rotation(plan): # find total joint rotation in successful trajectory angles = [0, 0, 0, 0, 0, 0] number_of_points = len(plan.joint_trajectory.points) for i in range(number_of_points - 1): angles_temp = [ abs(x - y) for x, y in zip(plan.joint_trajectory.points[i + 1].positions, plan.joint_trajectory.points[i].positions) ] angles = [x + y for x, y in zip(angles, angles_temp)] total_angle_change = sum(angles) return total_angle_change def _check_computation_time(self, msg): # get computation time for successful plan to be found if msg.status.status == 3: self._comp_time.append(msg.result.planning_time) def _check_path_length(self, plan): # find distance travelled by end effector # not yet implemented return
class MoveIt(object): def __init__(self): moveit_commander.roscpp_initialize(sys.argv) self.scene = PlanningSceneInterface() self.add_table() # self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.arm = MoveGroupCommander("arm") # self.arm.set_goal_joint_tolerance(0.1) self.gripper = MoveGroupCommander("gripper") # already default self.arm.set_planner_id("RRTConnectkConfigDefault") self.end_effector_link = self.arm.get_end_effector_link() self.arm.allow_replanning(True) self.arm.set_planning_time(5) self.transformer = tf.TransformListener() rospy.sleep(2) # allow some time for initialization of moveit def __del__(self): moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) def _open_gripper(self): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [0, 0] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory def _set_gripper_width(self, width): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [0, 0] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory def _close_gripper(self): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [1.2, 1.2] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory # Template function for creating the Grasps def _create_grasp(self, x, y, z, roll, pitch, yaw): grasp = Grasp() # pre_grasp grasp.pre_grasp_posture = self._open_gripper() grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link grasp.pre_grasp_approach.direction.vector.z = 1.0 grasp.pre_grasp_approach.direction.vector.y = 0.0 grasp.pre_grasp_approach.direction.vector.x = 0.0 grasp.pre_grasp_approach.min_distance = 0.05 grasp.pre_grasp_approach.desired_distance = 0.1 # grasp grasp.grasp_posture = self._close_gripper() grasp.grasp_pose.pose.position.x = x grasp.grasp_pose.pose.position.y = y grasp.grasp_pose.pose.position.z = z q = quaternion_from_euler(roll, pitch, yaw) grasp.grasp_pose.pose.orientation.x = q[0] grasp.grasp_pose.pose.orientation.y = q[1] grasp.grasp_pose.pose.orientation.z = q[2] grasp.grasp_pose.pose.orientation.w = q[3] grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # post_grasp grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link grasp.post_grasp_retreat.direction.vector.z = -1.0 grasp.post_grasp_retreat.direction.vector.x = 0.0 grasp.post_grasp_retreat.direction.vector.y = 0.0 grasp.post_grasp_retreat.min_distance = 0.05 grasp.post_grasp_retreat.desired_distance = 0.25 return [grasp] # Template function, you can add parameters if needed! def grasp(self, x, y, z, roll, pitch, yaw, width): self.add_object('object', [0.37, -0.24, 0.1, math.pi, 0., 0.], [0.1, 0.1, 0.1]) grasps = self._create_grasp(x, y, z, roll, pitch, yaw) result = self.arm.pick('object', grasps) self.remove_object() if result == MoveItErrorCodes.SUCCESS: print 'Success grasp' return True else: print 'Failed grasp' return False def open_fingers(self): self.gripper.set_joint_value_target([0.0, 0.0]) self.gripper.go(wait=True) rospy.sleep(2.0) def close_fingers(self): self.gripper.set_joint_value_target([1.3, 1.3]) self.gripper.go(wait=True) rospy.sleep(2.0) def move_to(self, x, y, z, roll, pitch, yaw, frame_id="m1n6s200_link_base"): q = quaternion_from_euler(roll, pitch, yaw) pose = PoseStamped() pose.header.frame_id = frame_id pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = z pose.pose.orientation.x = q[0] pose.pose.orientation.y = q[1] pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] self.arm.set_pose_target(pose, self.end_effector_link) plan = self.arm.plan() success = True success = self.arm.go(wait=True) self.arm.stop() self.arm.clear_pose_targets() return success def remove_object(self, object='object'): self.scene.remove_attached_object(self.end_effector_link, object) self.scene.remove_world_object(object) rospy.loginfo("Object removed") def add_object(self, name, pose, size): object_pose = PoseStamped() object_pose.header.frame_id = "m1n6s200_link_base" object_pose.pose.position.x = pose[0] object_pose.pose.position.y = pose[1] object_pose.pose.position.z = pose[2] q = quaternion_from_euler(*pose[3:]) object_pose.pose.orientation.x = q[0] object_pose.pose.orientation.y = q[1] object_pose.pose.orientation.z = q[2] object_pose.pose.orientation.w = q[3] self.scene.add_box(name, object_pose, size) def add_table(self): self.add_object('table', [0, 0, -0.005, 0, 0, 0], (2, 2, 0.01))
rospy.init_node('pumpkin_planning', anonymous=True) right_arm = MoveGroupCommander("right_arm") display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1) print right_arm.get_current_pose().pose wpose = geometry_msgs.msg.Pose() wpose.orientation.w = 0.0176318609849 wpose.orientation.x = 0.392651866792 wpose.orientation.y = 0.918465607415 wpose.orientation.z = 0.0439835989663 wpose.position.y = 0.227115629827 wpose.position.z = 1.32344046934 wpose.position.x = -0.358178766726 right_arm.set_pose_target(wpose) plan1 = right_arm.plan() right_arm.execute(plan1) print "============ Waiting while RVIZ executes plan1..." rospy.sleep(5) waypoints = [] waypoints.append(right_arm.get_current_pose().pose) print right_arm.get_current_pose().pose points = 20 for i in xrange(points): wpose = geometry_msgs.msg.Pose() wpose.orientation.w = waypoints[i-1].orientation.w + 0.5285 wpose.orientation.x = waypoints[i-1].orientation.x - 0.6736 wpose.orientation.y = waypoints[i-1].orientation.y - 1.638 wpose.orientation.z = waypoints[i-1].orientation.z - 0.308
rospy.init_node('pumpkin_planning', anonymous=True) right_arm = MoveGroupCommander("right_arm") display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1) print right_arm.get_current_pose().pose wpose = geometry_msgs.msg.Pose() wpose.orientation.w = 0.56984725052 wpose.orientation.x = -0.279552201843 wpose.orientation.y = -0.720240690777 wpose.orientation.z = -0.279960755942 wpose.position.y = 0.109418655245 wpose.position.z = 1.32343676946 wpose.position.x = 0.230101774989 right_arm.set_pose_target(wpose) plan1 = right_arm.plan() right_arm.execute(plan1) print "============ Waiting while RVIZ executes plan1..." rospy.sleep(5) waypoints = [] waypoints.append(right_arm.get_current_pose().pose) print right_arm.get_current_pose().pose points = 20 for i in xrange(points): wpose = geometry_msgs.msg.Pose() wpose.orientation.w = waypoints[i-1].orientation.w - 0.5285 wpose.orientation.x = waypoints[i-1].orientation.x + 0.6736 wpose.orientation.y = waypoints[i-1].orientation.y + 1.638
class MoveItDemo: def __init__(self): global obj_att # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ############## CLEAR THE SCENE ################ # planning_scene.world.collision_objects.remove('target') # Remove leftover objects from a previous run self.scene.remove_world_object('target') self.scene.remove_world_object('table') # self.scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, 'target') # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() initial_pose = PoseStamped() initial_pose.header.frame_id = 'gazebo_world' initial_pose.pose = target_pose.pose print "==================== Generating Transformations ===========================" #################### PRE GRASPING POSE ######################### M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T = np.dot(M1, M2) pre_grasping = deepcopy(target_pose) pre_grasping.pose.position.x = T[0,3] pre_grasping.pose.position.y = T[1,3] pre_grasping.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) pre_grasping.pose.orientation.x = quat[0] pre_grasping.pose.orientation.y = quat[1] pre_grasping.pose.orientation.z = quat[2] pre_grasping.pose.orientation.w = quat[3] pre_grasping.header.frame_id = 'gazebo_world' self.plan_exec(pre_grasping) #################### GRASPING POSE ######################### M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M3[0,3] = target_pose.pose.position.x M3[1,3] = target_pose.pose.position.y M3[2,3] = target_pose.pose.position.z M4 = transformations.euler_matrix(0, 1.57, 0) M4[0,3] = 0.0 # offset about x M4[1,3] = 0.0 # about y M4[2,3] = 0.18 # about z T2 = np.dot(M3, M4) grasping = deepcopy(target_pose) grasping.pose.position.x = T2[0,3] grasping.pose.position.y = T2[1,3] grasping.pose.position.z = T2[2,3] quat2 = transformations.quaternion_from_matrix(T2) grasping.pose.orientation.x = quat2[0] grasping.pose.orientation.y = quat2[1] grasping.pose.orientation.z = quat2[2] grasping.pose.orientation.w = quat2[3] grasping.header.frame_id = 'gazebo_world' self.plan_exec(grasping) #Close the gripper print "========== Waiting for gazebo to catch up ==========" self.close_gripper() #################### ATTACH OBJECT ###################### touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link'] #print touch_links self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links) # counter to let the planning scene know when to remove the object obj_att = 1 #self.scene.remove_world_object(target_id) #################### POST-GRASP RETREAT ######################### M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w]) M5[0,3] = initial_pose.pose.position.x M5[1,3] = initial_pose.pose.position.y M5[2,3] = initial_pose.pose.position.z M6 = transformations.euler_matrix(0, 1.57, 0) M6[0,3] = 0.0 # offset about x M6[1,3] = 0.0 # about y M6[2,3] = 0.3 # about z T3 = np.dot(M5, M6) post_grasping = deepcopy(initial_pose) post_grasping.pose.position.x = T3[0,3] post_grasping.pose.position.y = T3[1,3] post_grasping.pose.position.z = T3[2,3] quat3 = transformations.quaternion_from_matrix(T3) post_grasping.pose.orientation.x = quat3[0] post_grasping.pose.orientation.y = quat3[1] post_grasping.pose.orientation.z = quat3[2] post_grasping.pose.orientation.w = quat3[3] post_grasping.header.frame_id = 'gazebo_world' self.plan_exec(post_grasping) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.52 place_pose.pose.position.y = -0.48 place_pose.pose.position.z = 0.48 place_pose.pose.orientation.w = 1.0 n_attempts = 0 max_place_attempts = 2 # Generate valid place poses places = self.make_places(place_pose) success = False # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_place_attempts: for place in places: success = self.right_arm.place(target_id, place) if success: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) self.open_gripper() obj_att = None rospy.sleep(3) ## # Initialize the grasp object ## g = Grasp() ## grasps = [] ## # Set the first grasp pose to the input pose ## g.grasp_pose = pre_grasping ## g.allowed_touch_objects = [target_id] ## grasps.append(deepcopy(g)) ## right_arm.pick(target_id, grasps) # #Change the frame_id for the planning to take place! # #target_pose.header.frame_id = 'gazebo_world' # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################################################################## #Get pose from Gazebo def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg # Generate a list of possible place poses def make_places(self, init_pose): # Initialize the place location as a PoseStamped message place = PoseStamped() # Start with the input place pose place = init_pose # A list of x shifts (meters) to try x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] # A list of y shifts (meters) to try y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] # A list of pitch angles to try #pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02] pitch_vals = [0] # A list of yaw angles to try yaw_vals = [0] # A list to hold the places places = [] # Generate a place pose for each angle and translation for y in yaw_vals: for p in pitch_vals: for y in y_vals: for x in x_vals: place.pose.position.x = init_pose.pose.position.x + x place.pose.position.y = init_pose.pose.position.y + y # Create a quaternion from the Euler angles q = quaternion_from_euler(0, p, y) # Set the place pose orientation accordingly place.pose.orientation.x = q[0] place.pose.orientation.y = q[1] place.pose.orientation.z = q[2] place.pose.orientation.w = q[3] # Append this place pose to the list places.append(deepcopy(place)) # Return the list return places def plan_exec(self, pose): self.right_arm.clear_pose_targets() self.right_arm.set_pose_target(pose, GRIPPER_FRAME) self.right_arm.plan() rospy.sleep(5) self.right_arm.go(wait=True) def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it def scene_generator(self): # print obj_att global target_pose global target_id global target_size target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene if obj_att is None: self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') # Publish targe's frame #self.object_frames_pub.publish(target_pose) threading.Timer(0.5, self.scene_generator).start() # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p)
# Set the start pose start_pose = geometry_msgs.msg.Pose( ) # Find how to set to custom pose - right_hand_kinect_start # Values for right_hand_kinect_start start_pose.position.x = 0.0464712799415 start_pose.position.y = 0.0737200235193 start_pose.position.z = 1.3721234889 start_pose.orientation.x = 0.212880825044 start_pose.orientation.y = 0.806655581926 start_pose.orientation.z = 0.440611611646 start_pose.orientation.w = 0.331436169057 right_shoulder_to_wrist.set_pose_target(start_pose) plan_start = right_shoulder_to_wrist.plan( ) #Facing problems in planning with this mouve group rospy.sleep(5) right_shoulder_to_wrist.execute(plan_start) rospy.sleep(5) # Set the orientation for next pose next_pose.orientation.x = 0.5 next_pose.orientation.y = 0.5 next_pose.orientation.z = 0.5 next_pose.orientation.w = 0.5 # Subscriber rospy.Subscriber("/right_shoulder_to_wrist_transform", Float32MultiArray, right_shoulder_to_wrist_callback) # Start mimic
class MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() self.obj_att = None # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ############## CLEAR THE SCENE ################ # planning_scene.world.collision_objects.remove('target') # Remove leftover objects from a previous run self.scene.remove_world_object('target') self.scene.remove_world_object('table') # self.scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, 'target') # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() initial_pose = target_pose initial_pose.header.frame_id = 'gazebo_world' print "==================== Generating Transformations ===========================" #################### PRE GRASPING POSE ######################### # M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) # M1[0,3] = target_pose.pose.position.x # M1[1,3] = target_pose.pose.position.y # M1[2,3] = target_pose.pose.position.z # M2 = transformations.euler_matrix(0, 1.57, 0) # M2[0,3] = 0.0 # offset about x # M2[1,3] = 0.0 # about y # M2[2,3] = 0.25 # about z # T = np.dot(M1, M2) # pre_grasping = deepcopy(target_pose) # pre_grasping.pose.position.x = T[0,3] # pre_grasping.pose.position.y = T[1,3] # pre_grasping.pose.position.z = T[2,3] # quat = transformations.quaternion_from_matrix(T) # pre_grasping.pose.orientation.x = quat[0] # pre_grasping.pose.orientation.y = quat[1] # pre_grasping.pose.orientation.z = quat[2] # pre_grasping.pose.orientation.w = quat[3] # pre_grasping.header.frame_id = 'gazebo_world' # self.plan_exec(pre_grasping) ################# GENERATE GRASPS ################### # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Track success/failure and number of attempts for pick operation success = False n_attempts = 0 grasps = self.grasp_generator(initial_pose) possible_grasps = [] for grasp in grasps: self.gripper_pose_pub.publish(grasp) possible_grasps.append(grasp.pose) rospy.sleep(0.2) #print possible_grasps self.right_arm.pick(target_id, grasps) # target_name = target_id # group_name = GROUP_NAME_ARM # end_effector = GROUP_NAME_GRIPPER # attached_object_touch_links = ['r_forearm_link'] # #print (target_name, group_name, end_effector) # PickupGoal(target_name, group_name ,end_effector, possible_grasps ) # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################# FUNCTIONS ################################################################################# def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def grasp_generator(self, initial_pose): # Pitch angles to try pitch_vals = [1, 1.57,0, 1,2 ] # Yaw angles to try yaw_vals = [0]#, 1.57, -1.57] # A list to hold the grasps grasps = [] g = PoseStamped() g.header.frame_id = REFERENCE_FRAME g.pose = initial_pose.pose #g.pose.position.z += 0.18 # Generate a grasp for each pitch and yaw angle for y in yaw_vals: for p in pitch_vals: # Create a quaternion from the Euler angles q = transformations.quaternion_from_euler(0, p, y) # Set the grasp pose orientation accordingly g.pose.orientation.x = q[0] g.pose.orientation.y = q[1] g.pose.orientation.z = q[2] g.pose.orientation.w = q[3] # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps def plan_exec(self, pose): self.right_arm.clear_pose_targets() self.right_arm.set_pose_target(pose, GRIPPER_FRAME) self.right_arm.plan() rospy.sleep(5) self.right_arm.go(wait=True) def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it def scene_generator(self): # print obj_att global target_pose global target_id next_call = time.time() while True: next_call = next_call+1 target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') # obstacle1_id = 'obstacle1' # self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] # obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 if self.obj_att is None: # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) # obstacle1_pose = PoseStamped() # obstacle1_pose.header.frame_id = REFERENCE_FRAME # obstacle1_pose.pose = self.pwh.pose[self.o1id] # # Add the target object to the scene # scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') time.sleep(next_call - time.time()) #threading.Timer(0.5, self.scene_generator).start() # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p)
class MoveIt(object): def __init__(self): moveit_commander.roscpp_initialize(sys.argv) self.scene = PlanningSceneInterface() self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.arm = MoveGroupCommander("arm") # self.arm.set_goal_joint_tolerance(0.1) self.gripper = MoveGroupCommander("gripper") # already default self.arm.set_planner_id("RRTConnectkConfigDefault") self.end_effector_link = self.arm.get_end_effector_link() self.arm.allow_replanning(True) self.arm.set_planning_time(5) self.transformer = tf.TransformListener() rospy.sleep(2) # allow some time for initialization of moveit def __del__(self): moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) def _open_gripper(self): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [0, 0] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory def _close_gripper(self): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [1.2, 1.2] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory # Template function for creating the Grasps def _create_grasps(self, x, y, z, z_max, rotation): grasps = [] # You can create multiple grasps and add them to the grasps list grasp = Grasp() # create a new grasp # Set the pre grasp posture (the fingers) grasp.pre_grasp_posture = self._open_gripper() # Set the grasp posture (the fingers) grasp.grasp_posture = self._close_gripper() # Set the position of where to grasp grasp.grasp_pose.pose.position.x = x grasp.grasp_pose.pose.position.y = y grasp.grasp_pose.pose.position.z = z # Set the orientation of the end effector q = quaternion_from_euler(math.pi, 0.0, rotation) grasp.grasp_pose.pose.orientation.x = q[0] grasp.grasp_pose.pose.orientation.y = q[1] grasp.grasp_pose.pose.orientation.z = q[2] grasp.grasp_pose.pose.orientation.w = q[3] grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # Set the pre_grasp_approach grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link grasp.pre_grasp_approach.direction.vector.z = 1.0 grasp.pre_grasp_approach.direction.vector.y = 0.0 grasp.pre_grasp_approach.direction.vector.x = 0.0 grasp.pre_grasp_approach.min_distance = 0.05 grasp.pre_grasp_approach.desired_distance = 0.1 # # Set the post_grasp_approach grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link grasp.post_grasp_retreat.direction.vector.z = -1.0 grasp.post_grasp_retreat.direction.vector.x = 0.0 grasp.post_grasp_retreat.direction.vector.y = 0.0 grasp.post_grasp_retreat.min_distance = 0.05 grasp.post_grasp_retreat.desired_distance = 0.25 grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # setting the planning frame (Positive x is to the left, negative Y is to the front of the arm) grasps.append( grasp ) # add all your grasps in the grasps list, MoveIT will pick the best one for z_offset in np.arange(z + 0.02, z_max, 0.01): new_grasp = copy.deepcopy(grasp) new_grasp.grasp_pose.pose.position.z = z_offset grasps.append(new_grasp) return grasps # Template function, you can add parameters if needed! def grasp(self, x, y, z, z_max, rotation, size): print '******************* grasp' # Object distance: obj_dist = np.linalg.norm(np.asarray((x, y, z))) if obj_dist > 0.5: rospy.loginfo( "Object too far appart ({} m), skipping pick".format(obj_dist)) return False # Add collision object, easiest to name the object, "object" object_pose = PoseStamped() object_pose.header.frame_id = "m1n6s200_link_base" object_pose.pose.position.x = x object_pose.pose.position.y = y object_pose.pose.position.z = z q = quaternion_from_euler(math.pi, 0.0, rotation) object_pose.pose.orientation.x = q[0] object_pose.pose.orientation.y = q[1] object_pose.pose.orientation.z = q[2] object_pose.pose.orientation.w = q[3] self.scene.add_box("object", object_pose, size) rospy.sleep(0.5) self.clear_octomap() rospy.sleep(1.0) # Create and return grasps # z += size[2]/2 # Focus on the top of the object only # z += size[2]/2 + 0.02 # Focus on the top of the object only grasps = self._create_grasps(x, y, z, z_max, rotation) print '******************************************************************************' result = self.arm.pick( 'object', grasps) # Perform pick on "object", returns result print '******************************************************************************' # self.move_to(x, y, z + 0.15, rotation) if result == MoveItErrorCodes.SUCCESS: print 'Success grasp' return True else: print 'Failed grasp' return False def clear_object(self, x, y, z, z_max, rotation, size): print '******************* clear_object' self.move_to_waypoint() success = self.grasp(x, y, z, z_max, rotation, size) if success: self.move_to_waypoint() success = self.move_to_drop_zone() if success: print 'success move to drop zone' else: print 'failed move to drop zone' self.open_fingers() self.remove_object() rospy.sleep(1.0) self.close_fingers() return success def open_fingers(self): print '******************* open_fingers' self.gripper.set_joint_value_target([0.0, 0.0]) self.gripper.go(wait=True) rospy.sleep(2.0) def close_fingers(self): print '******************* close_fingers' self.gripper.set_joint_value_target([1.3, 1.3]) self.gripper.go(wait=True) rospy.sleep(2.0) def move_to(self, x, y, z, rotation, frame_id="m1n6s200_link_base"): print '******************* move_to' q = quaternion_from_euler(math.pi, 0.0, rotation) pose = PoseStamped() pose.header.frame_id = frame_id pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = z pose.pose.orientation.x = q[0] pose.pose.orientation.y = q[1] pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] self.arm.set_pose_target(pose, self.end_effector_link) plan = self.arm.plan() success = self.arm.go(wait=True) self.arm.stop() self.arm.clear_pose_targets() return success def move_to_waypoint(self): print '******************* move_to_waypoint' return self.move_to(0.35, 0, 0.25, 1.57) def rtb(self): print '******************* rtb' self.move_to_drop_zone() # pose = PoseStamped() # pose.header.frame_id = 'base_footprint' # pose.pose.position.x = -0.191258927921 # pose.pose.position.y = 0.1849306168113 # pose.pose.position.z = 0.813729734732 # pose.pose.orientation.x = -0.934842026356 # pose.pose.orientation.y = 0.350652799078 # pose.pose.orientation.z = -0.00168532388516 # pose.pose.orientation.w = 0.0557688079539 # # self.arm.set_pose_target(pose, self.end_effector_link) # plan = self.arm.plan() # self.arm.go(wait=True) # self.arm.stop() # self.arm.clear_pose_targets() def move_to_drop_zone(self): print '******************* move_to_drop_zone' pose = PoseStamped() pose.header.frame_id = "m1n6s200_link_base" pose.pose.position.x = 0.2175546259709541 pose.pose.position.y = 0.18347985269448372 pose.pose.position.z = 0.16757751444136426 pose.pose.orientation.x = 0.6934210704552356 pose.pose.orientation.y = 0.6589390059796749 pose.pose.orientation.z = -0.23223137602833943 pose.pose.orientation.w = -0.17616808290725341 self.arm.set_pose_target(pose, self.end_effector_link) plan = self.arm.plan() success = self.arm.go(wait=True) self.arm.stop() self.arm.clear_pose_targets() return success def print_position(self): pose = self.arm.get_current_pose() self.transformer.waitForTransform("m1n6s200_link_base", "base_footprint", rospy.Time.now(), rospy.Duration(10)) eef_pose = self.transformer.transformPose("m1n6s200_link_base", pose) orientation = eef_pose.pose.orientation orientation = [ orientation.x, orientation.y, orientation.z, orientation.w ] euler = euler_from_quaternion(orientation) print "z:", eef_pose.pose.position.x print "y:", eef_pose.pose.position.y print "z:", eef_pose.pose.position.z print "yaw (degrees):", math.degrees(euler[2]) def remove_object(self): self.scene.remove_attached_object(self.end_effector_link, "object") self.scene.remove_world_object("object") rospy.loginfo("Object removed")
if __name__ == '__main__': print "--- Straight line gesture ---" rospy.init_node('straight_line', anonymous=True) right_arm = MoveGroupCommander("right_arm") start_pose = geometry_msgs.msg.Pose() start_pose.position.x = -0.0206330384032 start_pose.position.y = 0.077582282778 start_pose.position.z = 1.39283677496 start_pose.orientation.x = 0.5 start_pose.orientation.y = 0.5 start_pose.orientation.z = 0.5 start_pose.orientation.w = 0.5 right_arm.set_pose_target(start_pose) plan_start = right_arm.plan() print "Plan start" rospy.sleep(5) right_arm.execute(plan_start) print "Execute start" rospy.sleep(5) end_pose = geometry_msgs.msg.Pose() end_pose.position.x = -0.0434279649929 end_pose.position.y = -0.0562017053887 end_pose.position.z = 1.48763433664 end_pose.orientation.x = 0.5 end_pose.orientation.y = 0.5 end_pose.orientation.z = 0.5 end_pose.orientation.w = 0.5
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') rospy.Subscriber('/aruco_single/pose', PoseStamped, self.pose_cb,queue_size=1) scene=PlanningSceneInterface() self.scene_pub=rospy.Publisher('planning_scene',PlanningScene) self.colors=dict() rospy.sleep(1) arm=MoveGroupCommander('arm') #gripper=MoveGroupCommander('gripper') end_effector_link=arm.get_end_effector_link() arm.set_goal_position_tolerance(0.005) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) #gripper.set_goal_position_tolerance(0.005) #gripper.set_goal_orientation_tolerance(0.025) #gripper.allow_replanning(True) reference_frame='base_link' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) #scene planning table_id='table' #cylinder_id='cylinder' box2_id='box2' target_id='target_object' #scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) scene.remove_world_object(target_id) rospy.sleep(2) table_ground=0.59 table_size=[0.5,1,0.01] #box1_size=[0.1,0.05,0.03] box2_size=[0.15,0.15,0.02] r_tool_size=[0.05,0.04,0.22] l_tool_size=[0.05,0.04,0.22] target_size=[0.05,0.05,0.1] table_pose=PoseStamped() table_pose.header.frame_id=reference_frame table_pose.pose.position.x=0.7 table_pose.pose.position.y=0.0 table_pose.pose.position.z=table_ground+table_size[2]/2.0 table_pose.pose.orientation.w=1.0 scene.add_box(table_id,table_pose,table_size) ''' box1_pose=PoseStamped() box1_pose.header.frame_id=reference_frame box1_pose.pose.position.x=0.7 box1_pose.pose.position.y=-0.2 box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0 box1_pose.pose.orientation.w=1.0 scene.add_box(box1_id,box1_pose,box1_size) ''' box2_pose=PoseStamped() box2_pose.header.frame_id=reference_frame box2_pose.pose.position.x=0.55 box2_pose.pose.position.y=-0.12 box2_pose.pose.position.z=table_ground+table_size[2]+box2_size[2]/2.0 box2_pose.pose.orientation.w=1.0 scene.add_box(box2_id,box2_pose,box2_size) target_pose=PoseStamped() target_pose.header.frame_id=reference_frame target_pose.pose.position.x=0.58 target_pose.pose.position.y=0.05 target_pose.pose.position.z=table_ground+table_size[2]+target_size[2]/2.0 target_pose.pose.orientation.x=0 target_pose.pose.orientation.y=0 target_pose.pose.orientation.z=0 target_pose.pose.orientation.w=1 scene.add_box(target_id,target_pose,target_size) #left gripper l_p=PoseStamped() l_p.header.frame_id=end_effector_link l_p.pose.position.x=0.00 l_p.pose.position.y=0.06 l_p.pose.position.z=0.11 l_p.pose.orientation.w=1 scene.attach_box(end_effector_link,'l_tool',l_p,l_tool_size) #right gripper r_p=PoseStamped() r_p.header.frame_id=end_effector_link r_p.pose.position.x=0.00 r_p.pose.position.y= -0.06 r_p.pose.position.z=0.11 r_p.pose.orientation.w=1 scene.attach_box(end_effector_link,'r_tool',r_p,r_tool_size) #grasp g_p=PoseStamped() g_p.header.frame_id=end_effector_link g_p.pose.position.x=0.00 g_p.pose.position.y= -0.00 g_p.pose.position.z=0.025 g_p.pose.orientation.w=0.707 g_p.pose.orientation.x=0 g_p.pose.orientation.y=-0.707 g_p.pose.orientation.z=0 self.setColor(table_id,0.8,0,0,1.0) #self.setColor(box1_id,0.8,0.4,0,1.0) self.setColor(box2_id,0.8,0.4,0,1.0) self.setColor('r_tool',0.8,0,0) self.setColor('l_tool',0.8,0,0) self.setColor('target_object',0,1,0) self.sendColors() #motion planning arm.set_named_target("initial_arm") arm.go() rospy.sleep(2) grasp_pose=target_pose grasp_pose.pose.position.x-=0.15 #grasp_pose.pose.position.z= grasp_pose.pose.orientation.x=0 grasp_pose.pose.orientation.y=0.707 grasp_pose.pose.orientation.z=0 grasp_pose.pose.orientation.w=0.707 #arm.set_start_state_to_current_state() ''' arm.set_pose_target(grasp_pose,end_effector_link) traj=arm.plan() arm.execute(traj) print arm.get_current_joint_values() ''' pre_joint_state=[0.16588150906995922, 1.7060146047438647, -0.00961761728757362, 1.8614674591892713, -2.9556667436476847, 1.7432451233907822, 3.1415] arm.set_joint_value_target(pre_joint_state) traj=arm.plan() arm.execute(traj) rospy.sleep(2) arm.shift_pose_target(0,0.09,end_effector_link) arm.go() rospy.sleep(2) scene.attach_box(end_effector_link,target_id,g_p,target_size) rospy.sleep(2) #grasping is over , from now is pouring arm.shift_pose_target(2,0.15,end_effector_link) arm.go() rospy.sleep(2) joint_state_1=arm.get_current_joint_values() joint_state_1[0]-=0.17 arm.set_joint_value_target(joint_state_1) arm.go() rospy.sleep(1) joint_state_2=arm.get_current_joint_values() joint_state_2[6]-=1.8 arm.set_joint_value_target(joint_state_2) arm.go() rospy.sleep(1) #print arm.get_current_joint_values() #pouring test for i in range(1,5): joint_state_2[6]+=0.087 arm.set_joint_value_target(joint_state_2) arm.go() time.sleep(0.05) joint_state_2[6]-=0.087 arm.set_joint_value_target(joint_state_2) arm.go() time.sleep(0.05) print i joint_state_2[6]+=1.8 arm.set_joint_value_target(joint_state_2) arm.go() rospy.sleep(2) joint_state_1[0]+=0.17 arm.set_joint_value_target(joint_state_1) arm.go() rospy.sleep(1) arm.shift_pose_target(2,-0.15,end_effector_link) arm.go() rospy.sleep(2) scene.remove_attached_object(end_effector_link,target_id) rospy.sleep(2) arm.set_named_target("initial_arm") arm.go() rospy.sleep(2) #remove and shut down scene.remove_attached_object(end_effector_link,'l_tool') rospy.sleep(1) scene.remove_attached_object(end_effector_link,'r_tool') rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class SrFastGrasp: def __init__(self): self.__marker_pub = rospy.Publisher("visualization_marker", Marker, queue_size=1) self.__grasp_server = rospy.Service("grasp_from_bounding_box", GetFastGraspFromBoundingBox, self.__bounding_box_cb) self.__default_grasp = 'super_amazing_grasp' self.__get_state = rospy.ServiceProxy( '/grasp_warehouse/get_robot_state', GetState) hand_group = rospy.get_param("~hand_group", "right_hand") arm_group = rospy.get_param("~arm_group", "right_arm") self.__group = MoveGroupCommander(hand_group) self.__arm_g = MoveGroupCommander(arm_group) self.__ik = rospy.ServiceProxy("compute_ik", GetPositionIK) def __modify_grasp_pose(self, grasp, pose): """ Aligns grasp with axis from origin to center of object. A crude way to make a vaguely sane orientation for the hand that seems to more or less work. """ v1 = numpy.array( [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]) v1_length = numpy.linalg.norm(v1) v1 = v1 / v1_length v2 = [1, 0, -v1[0] / v1[2]] v2 = v2 / numpy.linalg.norm(v2) v3 = numpy.cross(v1, v2) v3 = v3 / numpy.linalg.norm(v3) m = [[v3[0], v1[0], v2[0]], [v3[1], v1[1], v2[1]], [v3[2], v1[2], v2[2]]] q = quaternion_from_matrix(m) grasp.grasp_pose = deepcopy(pose) grasp.grasp_pose.pose.orientation.x = q[0] grasp.grasp_pose.pose.orientation.y = q[1] grasp.grasp_pose.pose.orientation.z = q[2] grasp.grasp_pose.pose.orientation.w = q[3] def __bounding_box_cb(self, request): box = request.bounding_box pose = request.pose if SolidPrimitive.BOX != box.type: rospy.logerr("Bounding volume must be a BOX.") return None self.__send_marker_to_rviz(box, pose) grasp_name = self.__select_grasp() grasp = self.__get_grasp(grasp_name) self.__modify_grasp_pose(grasp, pose) return grasp def __select_grasp(self): return self.__default_grasp def __get_grasp(self, name): try: open_state = self.__get_state(name + "_open", "").state closed_state = self.__get_state(name + "_closed", "").state except Exception: rospy.logfatal("Couldn't get grasp pose from db.") return Grasp() try: self.__group.set_start_state_to_current_state() pre_pose = self.__group.plan(open_state.joint_state) self.__group.set_start_state(open_state) pose = self.__group.plan(closed_state.joint_state) except Exception: rospy.logfatal("Couldn't plan grasp trajectories.") return Grasp() grasp = Grasp() grasp.id = name grasp.pre_grasp_posture = pre_pose.joint_trajectory grasp.grasp_posture = pose.joint_trajectory grasp.pre_grasp_approach.desired_distance = 0.2 grasp.pre_grasp_approach.min_distance = 0.1 grasp.pre_grasp_approach.direction.vector.x = 0 grasp.pre_grasp_approach.direction.vector.y = -1 grasp.pre_grasp_approach.direction.vector.z = 0 return grasp def __get_major_axis(self, box): m = max(box.dimensions) max_index = [i for i, j in enumerate(box.dimensions) if j == m] return max_index[-1] # Get the LAST axis with max val. def __send_marker_to_rviz(self, box, pose): marker = self.__get_marker_from_box(box, pose) self.__marker_pub.publish(marker) def __get_marker_from_box(self, box, pose): marker = Marker() marker.pose = pose.pose marker.header.frame_id = pose.header.frame_id marker.scale.x = box.dimensions[SolidPrimitive.BOX_X] marker.scale.y = box.dimensions[SolidPrimitive.BOX_Y] marker.scale.z = box.dimensions[SolidPrimitive.BOX_Z] marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 0.5 marker.lifetime = rospy.rostime.Duration() marker.type = Marker.CUBE marker.ns = "sr_fast_grasp_target" marker.id = 0 marker.action = Marker.ADD return marker
if __name__ == '__main__': roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() arm = MoveGroupCommander("manipulator") eef = MoveGroupCommander("endeffector") rospy.sleep(1) #Start State Gripper group_variable_values = eef.get_current_joint_values() group_variable_values[0] = 0.0 eef.set_joint_value_target(group_variable_values) plan2 = eef.plan() arm.execute(plan2) # clean the scene scene.remove_world_object("pole") scene.remove_world_object("table") scene.remove_world_object("part") # publish a demo scene k = PoseStamped() k.header.frame_id = robot.get_planning_frame() k.pose.position.x = 0.0 k.pose.position.y = 0.0 k.pose.position.z = -0.05 scene.add_box("table", k, (2, 2, 0.0001)) p = PoseStamped() p.header.frame_id = robot.get_planning_frame()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ## Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Prepare Gripper and open it self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1)) self.ac.send_goal(g_open) # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) rospy.sleep(2) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Remove leftover objects from a previous run scene.remove_world_object(target_id) scene.remove_world_object(table_id) #scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() #print target_pose self.object_frames_pub.publish(target_pose) rospy.sleep(2) print "==================== Generating Transformations ===========================" M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T = np.dot(M1, M2) pre_grasping = deepcopy(target_pose) pre_grasping.pose.position.x = T[0,3] pre_grasping.pose.position.y = T[1,3] pre_grasping.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) pre_grasping.pose.orientation.x = quat[0] pre_grasping.pose.orientation.y = quat[1] pre_grasping.pose.orientation.z = quat[2] pre_grasping.pose.orientation.w = quat[3] pre_grasping.header.frame_id = 'gazebo_world' # # Initialize the grasp object # g = Grasp() # grasps = [] # # Set the first grasp pose to the input pose # g.grasp_pose = pre_grasping # g.allowed_touch_objects = [target_id] # grasps.append(deepcopy(g)) # right_arm.pick(target_id, grasps) #Change the frame_id for the planning to take place! #target_pose.header.frame_id = 'gazebo_world' self.p_pub.publish(pre_grasping) right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME) plan = right_arm.plan() rospy.sleep(2) right_arm.go(wait=True) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
p5.header.frame_id = robot.get_planning_frame() p5.pose.position.x = -0.6 p5.pose.position.y = 0.7 p5.pose.position.z = 0.1 - 0.025 + 0.105 scene.add_box("foor2", p5, (0.2, 0.4, 0.01)) rospy.sleep(1) temPose = p temPose.pose.position.z -= 0.01 temPose.pose.position.x += 0.125 temPose.pose.orientation.y = 1 # temPose.pose.orientation.w = 0.707 print temPose arm.set_pose_target(temPose) pl = arm.plan() state = arm.execute(pl) print state rospy.sleep(1) if (state): cnt = -1 while cnt < 7: client.wait_for_server() goal = GripperCommandAction().action_goal.goal if cnt < 0: goal.command.position = 0.35 else: goal.command.position = 0.445 - (2**(6 - cnt)) * 0.001 rospy.loginfo(goal.command.position) rospy.sleep(1)
class CalibrationMovements: def __init__(self, move_group_name, max_velocity_scaling=0.5, max_acceleration_scaling=0.5): #self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic self.mgc = MoveGroupCommander(move_group_name) self.mgc.set_planner_id("RRTConnectkConfigDefault") self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling) self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling) self.start_pose = self.mgc.get_current_pose() self.poses = [] self.current_pose_index = -1 self.fallback_joint_limits = [math.radians(90)] * 4 + [ math.radians(90) ] + [math.radians(180)] + [math.radians(350)] if len(self.mgc.get_active_joints()) == 6: self.fallback_joint_limits = self.fallback_joint_limits[1:] def compute_poses_around_current_state(self, angle_delta, translation_delta): self.start_pose = self.mgc.get_current_pose() basis = np.eye(3) pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(izip(pos_deltas, neg_deltas))) # interleave final_rots = [] for qd in quaternion_deltas: final_rots.append(list(qd)) # TODO: clean up pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta / 2) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta / 2)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(izip(pos_deltas, neg_deltas))) # interleave for qd in quaternion_deltas: final_rots.append(list(qd)) final_poses = [] for rot in final_rots: fp = deepcopy(self.start_pose) ori = fp.pose.orientation combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w], rot) fp.pose.orientation = Quaternion(*combined_rot) final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.x += translation_delta / 2 final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.x -= translation_delta / 2 final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.y += translation_delta final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.y -= translation_delta final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.z += translation_delta / 3 final_poses.append(fp) self.poses = final_poses self.current_pose_index = -1 def check_poses(self, joint_limits): if len(self.fallback_joint_limits) == 6: joint_limits = joint_limits[1:] for fp in self.poses: self.mgc.set_pose_target(fp) plan = self.mgc.plan() if len(plan.joint_trajectory.points ) == 0 or CalibrationMovements.is_crazy_plan( plan, joint_limits): return False return True def plan_to_start_pose(self): return self.plan_to_pose(self.start_pose) def plan_to_pose(self, pose): self.mgc.set_start_state_to_current_state() self.mgc.set_pose_target(pose) plan = self.mgc.plan() return plan def execute_plan(self, plan): if CalibrationMovements.is_crazy_plan(plan, self.fallback_joint_limits): raise RuntimeError("got crazy plan!") self.mgc.execute(plan) @staticmethod def rot_per_joint(plan, degrees=False): np_traj = np.array([p.positions for p in plan.joint_trajectory.points]) if len(np_traj) == 0: raise ValueError np_traj_max_per_joint = np_traj.max(axis=0) np_traj_min_per_joint = np_traj.min(axis=0) ret = abs(np_traj_max_per_joint - np_traj_min_per_joint) if degrees: ret = [math.degrees(j) for j in ret] return ret @staticmethod def is_crazy_plan(plan, max_rotation_per_joint): abs_rot_per_joint = CalibrationMovements.rot_per_joint(plan) if (abs_rot_per_joint > max_rotation_per_joint).any(): return True else: return False
# arm_group.set_goal_position_tolerance(0.001) q = quaternion_from_euler(1.5701, 0.0, -1.5701) pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = q[0] pose_target.orientation.y = q[1] pose_target.orientation.z = q[2] pose_target.orientation.w = q[3] pose_target.position.z = 0.10 #0.25 pose_target.position.y = 0.00 #0.00 pose_target.position.x = -0.45 #-0.45 arm_group.set_pose_target(pose_target) # # # ## Now, we call the planner to compute the plan and execute it. plan = arm_group.plan(pose_target) while plan[0] != True: plan = arm_group.plan(pose_target) if plan[0]: traj = plan[1] arm_group.execute(traj, wait=True) arm_group.stop() arm_group.clear_pose_targets() # rospy.sleep(1) #GO FRONT
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) self.colors = dict() rospy.sleep(1) arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() arm.set_goal_position_tolerance(0.005) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) #scene planning table_id = 'table' #cylinder_id='cylinder' #box1_id='box1' box2_id = 'box2' target_id = 'target_object' #scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) scene.remove_world_object(target_id) rospy.sleep(2) table_ground = 0.68 table_size = [0.5, 1, 0.01] #box1_size=[0.1,0.05,0.03] box2_size = [0.05, 0.05, 0.1] r_tool_size = [0.03, 0.01, 0.06] l_tool_size = [0.03, 0.01, 0.06] target_size = [0.05, 0.05, 0.1] table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.75 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) ''' box1_pose=PoseStamped() box1_pose.header.frame_id=reference_frame box1_pose.pose.position.x=0.7 box1_pose.pose.position.y=-0.2 box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0 box1_pose.pose.orientation.w=1.0 scene.add_box(box1_id,box1_pose,box1_size) ''' box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.6 box2_pose.pose.position.y = -0.05 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.6 target_pose.pose.position.y = 0.05 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.x = 0 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = 0 target_pose.pose.orientation.w = 1 scene.add_box(target_id, target_pose, target_size) #left gripper l_p = PoseStamped() l_p.header.frame_id = end_effector_link l_p.pose.position.x = 0.00 l_p.pose.position.y = 0.04 l_p.pose.position.z = 0.04 l_p.pose.orientation.w = 1 scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size) #right gripper r_p = PoseStamped() r_p.header.frame_id = end_effector_link r_p.pose.position.x = 0.00 r_p.pose.position.y = -0.04 r_p.pose.position.z = 0.04 r_p.pose.orientation.w = 1 scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size) #grasp g_p = PoseStamped() g_p.header.frame_id = end_effector_link g_p.pose.position.x = 0.00 g_p.pose.position.y = -0.00 g_p.pose.position.z = 0.025 g_p.pose.orientation.w = 0.707 g_p.pose.orientation.x = 0 g_p.pose.orientation.y = -0.707 g_p.pose.orientation.z = 0 #set color self.setColor(table_id, 0.8, 0, 0, 1.0) #self.setColor(box1_id,0.8,0.4,0,1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) self.setColor('r_tool', 0.8, 0, 0) self.setColor('l_tool', 0.8, 0, 0) self.setColor('target_object', 0, 1, 0) self.sendColors() #motion planning arm.set_named_target("initial_arm1") arm.go() rospy.sleep(2) grasp_pose = target_pose grasp_pose.pose.position.x -= 0.15 #grasp_pose.pose.position.z= grasp_pose.pose.orientation.x = 0 grasp_pose.pose.orientation.y = 0.707 grasp_pose.pose.orientation.z = 0 grasp_pose.pose.orientation.w = 0.707 arm.set_start_state_to_current_state() arm.set_pose_target(grasp_pose, end_effector_link) traj = arm.plan() arm.execute(traj) rospy.sleep(2) print arm.get_current_joint_values() #arm.shift_pose_target(4,1.57,end_effector_link) #arm.go() #rospy.sleep(2) arm.shift_pose_target(0, 0.11, end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() saved_target_pose = arm.get_current_pose(end_effector_link) #arm.set_named_target("initial_arm2") #grasp scene.attach_box(end_effector_link, target_id, g_p, target_size) rospy.sleep(2) #grasping is over , from now is placing arm.shift_pose_target(2, 0.15, end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() arm.shift_pose_target(1, -0.2, end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() arm.shift_pose_target(2, -0.15, end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() scene.remove_attached_object(end_effector_link, target_id) rospy.sleep(2) #arm.set_pose_target(saved_target_pose,end_effector_link) #arm.go() #rospy.sleep(2) arm.set_named_target("initial_arm1") arm.go() rospy.sleep(2) #remove and shut down scene.remove_attached_object(end_effector_link, 'l_tool') scene.remove_attached_object(end_effector_link, 'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class BaxterMoveit(BaxterRobot): LEFT = -1 RIGHT = 1 def __init__(self, node, limb=+1): super(BaxterMoveit, self).__init__(node, limb=+1) self.group = MoveGroupCommander(self.lns + "_arm") self.robot = RobotCommander() self.scene = PlanningSceneInterface() self._info() def _info(self): '''Getting Basic Information''' # name of the reference frame for this robot: print("@@@@@@@@@@@@ Reference frame: %s" % self.group.get_planning_frame()) # We can also print the name of the end-effector link for this group: print("@@@@@@@@@@@@ End effector: %s" % self.group.get_end_effector_link()) # We can get a list of all the groups in the robot: print("@@@@@@@@@@@@ Robot Groups: @@@@@@@@@@@@@", self.robot.get_group_names()) # Sometimes for debugging it is useful to print the entire state of the # robot: print("@@@@@@@@@@@@ Printing robot state @@@@@@@@@@@@@") print(self.robot.get_current_state()) def obstacle(self, name, position, size): planning_frame = self.robot.get_planning_frame() pose = PoseStamped() pose.header.frame_id = planning_frame pose.pose.position.x = position[0] pose.pose.position.y = position[1] pose.pose.position.z = position[2] self.scene.add_box(name, pose, tuple(size)) def movej(self, q, raw=False): ''' move in joint space by giving a joint configuration as dictionary''' if raw: print("in moveit 'raw' motion is not avaiable") # succ = False # while succ is False: succ = self.group.go(q, wait=True) # self.group.stop() # ensures that there is no residual movement def showplan(self, target): if type(target) is PyKDL.Frame or type(target) is Pose: q = self.ik(target) elif type(target) is dict: q = target else: print("Target format error") return self.group.set_joint_value_target(q) self.group.plan() def movep(self, pose, raw=False): ''' move the eef in Cartesian space by giving a geometry_msgs.Pose or a PyKDL.Frame''' if type(pose) is PyKDL.Frame: pose = transformations.KDLToPose(pose) q = self.ik(pose) if q is not None: self.movej(q, raw=raw) else: print("\nNO SOLUTION TO IK\n" * 20)
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) #Call the planner to compute the plan and visualize it if successful. print "============ Generating plan for left arm" pose_target_left = geometry_msgs.msg.Pose() pose_target_left.orientation.x = 0.69283 pose_target_left.orientation.y = 0.1977 pose_target_left.orientation.z = -0.16912 pose_target_left.orientation.w = 0.67253 pose_target_left.position.x = 0.81576 pose_target_left.position.y = 0.093893 pose_target_left.position.z = 0.2496 group_left_arm.set_pose_target(pose_target_left) #group_left_arm.set_position_target([0.75,0.27,0.35]) plan_1eft = group_left_arm.plan() #print "Trajectory time (nsec): ", plan_left.joint_trajectory.points[len(plan_left.joint_trajectory.points)-1].time_from_start rospy.sleep(5) print "============ Generating plan for right arm" pose_target_right = geometry_msgs.msg.Pose() pose_target_right.orientation.x = 0.56508 pose_target_right.orientation.y = -0.5198 pose_target_right.orientation.z = -0.54332 pose_target_right.orientation.w = -0.33955 pose_target_right.position.x = 0.72651 pose_target_right.position.y = -0.041037 pose_target_right.position.z = 0.19097 group_right_arm.set_pose_target(pose_target_right) #group_right_arm.set_position_target([0.75,-0.27,0.35])