class Baxterpicknumber(): def __init__(self): rospy.loginfo("=============Ready to move the robot================") #Initialize moveit_commander self.robot=moveit_commander.RobotCommander() self.scene=moveit_commander.PlanningSceneInterface() self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm") #Display Trajectory in RVIZ self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) rospy.sleep(3.0) self.planning_frame = self.right_arm_group.get_planning_frame() rospy.loginfo( "============ Reference frame: %s" % self.planning_frame) self.eef_link = self.right_arm_group.get_end_effector_link() print "============ End effector: %s" % self.eef_link self.group_names = self.robot.get_group_names() print "============ Robot Groups:", self.robot.get_group_names() #set planning and execution parameters self.right_arm_group.set_goal_position_tolerance(0.01) self.right_arm_group.set_goal_orientation_tolerance(0.01) self.right_arm_group.set_planning_time(5.0) self.right_arm_group.allow_replanning(False) self.right_arm_group.set_max_velocity_scaling_factor(0.5) self.right_arm_group.set_max_acceleration_scaling_factor(0.5) #specify which gripper and limb are taking command self.right_gripper = Gripper('right', CHECK_VERSION) self.right_gripper.reboot() self.right_gripper.calibrate() self.pose_target=Pose() self.startpose=Pose() self.standoff=0.2 #Where to place the block self.place_target=Pose() self.placex_coord= 0.65 self.placey_coord=0 self.__right_sensor=rospy.Subscriber('/robot/range/right_hand_range/state',Range,self.rangecallback) # self.location_data=rospy.Subscriber('square_location',String,self.compute_cartesian) self.compute_cartesian() self.rangestatetemp=Range() def rangecallback(self, data): self.rangestatetemp=data self.rangestate=self.rangestatetemp.range def go_startpose(self): self.initialpose = {'right_s0': 0.03413107253045045, 'right_s1': -0.6937428113211783, 'right_e0': 0.1277039005914607, 'right_e1': 0.9671748867617533, 'right_w0': -0.16835439147042416, 'right_w1': 1.0810729602622453, 'right_w2': -0.1472621556369997} self.right_arm_group.set_joint_value_target(self.initialpose) rospy.loginfo("Attempting to start pose") plan=self.right_arm_group.plan() self.right_arm_group.go() self.startpose.position.x=0.693292944176 self.startpose.position.y=-0.81004861946 self.startpose.position.z=0.0914586599661 self.startpose.orientation.x=0.235668914045 self.startpose.orientation.y=0.964651313563 self.startpose.orientation.z=-0.0681154565907 self.startpose.orientation.w=0.0962719625176 def compute_cartesian(self):#,data): testingdata=np.array([0.738492350508,-0.320147457674,-0.209,np.pi]) data=testingdata # data=data.data.split('&') rospy.loginfo("=================Received desired position==============") #extract x,y,z coordinate from subscriber data self.x_coord=data[0] self.y_coord=data[1] self.z_coord=data[2] theta= data[3] #put coordinate data back to arrays pout=np.array([self.x_coord,self.y_coord,self.z_coord]) block_orientation=tr.quaternion_from_euler(theta,0,0,'sxyz') #convert orentation and position to Quaternion quat=Quaternion(*block_orientation) self.quat_position=Point(*pout) self.quat_orientation= copy.deepcopy(quat) self.pose_target.position=Point(*pout) self.pose_target.orientation=self.quat_orientation #Quaternian location for place area placepout=np.array([self.placex_coord,self.placey_coord,self.z_coord]) self.place_target.position=Point(*placepout) self.place_target.orientation=copy.deepcopy(quat) def move_arm_standoff(self): # self.moving='1' # self.moving_publisher.publish(self.moving) rospy.loginfo("==================Going to standoff posistion==============") z_standoff=self.z_coord+self.standoff waypoints=[] ite=30 xite=np.linspace(self.startpose.position.x,self.x_coord,ite) yite=np.linspace(self.startpose.position.y,self.y_coord,ite) zite=np.linspace(self.startpose.position.z,z_standoff,ite) # xoite=np.linspace(self.startpose.orientation.x,self.quat_orientation.x,ite) # yoite=np.linspace(self.startpose.orientation.y,self.quat_orientation.y,ite) # zoite=np.linspace(self.startpose.orientation.z,self.quat_orientation.z,ite) # woite=np.linspace(self.startpose.orientation.w,self.quat_orientation.w,ite) for i in range(ite): p=copy.deepcopy(self.startpose) p.position.x=xite[i] p.position.y=yite[i] p.position.z=zite[i] # p.orientation.x=xoite[i] # p.orientation.y=yoite[i] # p.orientation.z=zoite[i] # p.orientation.w=woite[i] waypoints.append(p) # print(waypoints) rospy.sleep(1.0) self.right_arm_group.set_start_state_to_current_state() fraction = 0.0 max_attempts = 200 attempts = 0 # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < max_attempts: (plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0) # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") self.right_arm_group.execute(plan) else: rospy.logerr("Could not find valid cartesian path") self.go_startpose() # self.move_arm_pick() def move_arm_pick(self): rospy.loginfo("====================moving to pick===================") waypoints=[] z_standoff=self.z_coord+self.standoff ite=30 zite=np.linspace(z_standoff,self.z_coord,ite) waypoints=[] # print(zite) for i in range(ite): p=copy.deepcopy(self.pose_target) p.position.z=zite[i] waypoints.append(p) # print(waypoints) rospy.sleep(1.0) self.right_arm_group.set_start_state_to_current_state() fraction = 0.0 max_attempts = 200 attempts = 0 # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < max_attempts: (plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0) # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") self.right_arm_group.execute(plan) else: rospy.logerr("Could not find valid cartesian path") # print(self.rangestate) if self.rangestate < 0.4: self.right_gripper.close() else: self.right_gripper.open() # self.right_arm_group.stop() # self.right_arm_group.clear_pose_targets() # rospy.sleep(1.0) def move_arm_backstandoff(self): self.right_gripper.close() rospy.loginfo('====================moving back to standoff=================') waypoints=[] z_standoff=self.z_coord+self.standoff ite=30 zite=np.linspace(self.z_coord,z_standoff,30) waypoints=[] # print(zite) for i in range(ite): p=copy.deepcopy(self.pose_target) p.position.z=zite[i] waypoints.append(p) # print(waypoints) rospy.sleep(1.0) self.right_arm_group.set_start_state_to_current_state() fraction = 0.0 max_attempts = 200 attempts = 0 # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < max_attempts: (plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0) # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") self.right_arm_group.execute(plan) else: rospy.logerr("Could not find valid cartesian path") def move_arm_standoff2(self): rospy.loginfo('==================moving to place standoff position==================') waypoints=[] z_standoff=self.z_coord+self.standoff ite=30 xite=np.linspace(self.x_coord,self.placex_coord,ite) yite=np.linspace(self.y_coord,self.placey_coord,ite) waypoints=[] # print(zite) for i in range(ite): p=copy.deepcopy(self.place_target) p.position.z=z_standoff p.position.x=xite[i] p.position.y=yite[i] waypoints.append(p) # print(waypoints) rospy.sleep(1.0) self.right_arm_group.set_start_state_to_current_state() fraction = 0.0 max_attempts = 200 attempts = 0 # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < max_attempts: (plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0) # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") self.right_arm_group.execute(plan) else: rospy.logerr("Could not find valid cartesian path") def move_arm_place(self): rospy.loginfo('===================moving to place the block===================') waypoints=[] z_standoff=self.z_coord+self.standoff ite=30 zite=np.linspace(z_standoff,self.z_coord,ite) waypoints=[] # print(zite) for i in range(ite): p=copy.deepcopy(self.place_target) p.position.z=zite[i] waypoints.append(p) # print(waypoints) rospy.sleep(1.0) self.right_arm_group.set_start_state_to_current_state() fraction = 0.0 max_attempts = 200 attempts = 0 # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < max_attempts: (plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0) # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") self.right_arm_group.execute(plan) else: rospy.logerr("Could not find valid cartesian path") self.right_gripper.open() def move_arm_backstandoff2(self): rospy.loginfo('=============moving to place standoff position=================') waypoints=[] z_standoff=self.z_coord+self.standoff ite=30 zite=np.linspace(self.z_coord,z_standoff,ite) waypoints=[] # print(zite) for i in range(ite): p=copy.deepcopy(self.place_target) p.position.z=zite[i] waypoints.append(p) # print(waypoints) rospy.sleep(1.0) self.right_arm_group.set_start_state_to_current_state() fraction = 0.0 max_attempts = 200 attempts = 0 # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < max_attempts: (plan, fraction) = self.right_arm_group.compute_cartesian_path(waypoints, 0.01, 0.0) # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") self.right_arm_group.execute(plan) else: rospy.logerr("Could not find valid cartesian path") self.go_startpose()
class Baxterpicknumber(): def __init__(self): global place_areax global place_areay global placetimes rospy.loginfo("=============Ready to move the robot================") #Initialize moveit_commander self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm") self.left_arm_group = moveit_commander.MoveGroupCommander("left_arm") #Display Trajectory in RVIZ self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.moving_publisher = rospy.Publisher('amimoving', String, queue_size=10) rospy.sleep(3.0) self.planning_frame = self.right_arm_group.get_planning_frame() rospy.loginfo("============ Reference frame: %s" % self.planning_frame) self.eef_link = self.right_arm_group.get_end_effector_link() print "============ End effector: %s" % self.eef_link self.group_names = self.robot.get_group_names() print "============ Robot Groups:", self.robot.get_group_names() #set planning and execution parameters self.right_arm_group.set_goal_position_tolerance(0.01) self.right_arm_group.set_goal_orientation_tolerance(0.01) self.right_arm_group.set_planning_time(5.0) self.right_arm_group.allow_replanning(False) self.right_arm_group.set_max_velocity_scaling_factor(1) self.right_arm_group.set_max_acceleration_scaling_factor(1) self.left_arm_group.set_goal_position_tolerance(0.01) self.left_arm_group.set_goal_orientation_tolerance(0.01) self.left_arm_group.set_planning_time(5.0) self.left_arm_group.allow_replanning(False) self.left_arm_group.set_max_velocity_scaling_factor(0.5) self.left_arm_group.set_max_acceleration_scaling_factor(0.5) #specify which gripper and limb are taking command self.right_gripper = Gripper('right', CHECK_VERSION) self.right_gripper.reboot() self.right_gripper.calibrate() # self.left_gripper = Gripper('left', CHECK_VERSION) # self.left_gripper.reboot() # self.left_gripper.calibrate() #Standoff hight above the block self.standoff = 0.2 #Screw axis for Right arm of Baxter self.Blist = np.array([[-1, 0, 0, 0, 1.17594, 0], [0, 1, 0, 1.10694, 0, -0.079], [0, 0, 1, 0, 0.079, 0], [0, 1, 0, 0.74259, 0, -0.01], [0, 0, 1, 0, 0.01, 0], [0, 1, 0, 0.3683, 0, 0], [0, 0, 1, 0, 0, 0]]).T self.M = np.array( [[0, 1 / sqrt(2), 1 / sqrt(2), 0.064 + (1.17594 / sqrt(2))], [0, 1 / sqrt(2), -1 / sqrt(2), -0.278 - (1.17594 / sqrt(2))], [-1, 0, 0, 0.19135], [0, 0, 0, 1]]) #subscribe to range sensor topic self.__right_sensor = rospy.Subscriber( '/robot/range/right_hand_range/state', Range, self.rangecallback) self.rangestatetemp = Range() #subscribe to target location topic self.grabcamera() self.go_startpose() self.location_data = rospy.Subscriber('square_location', String, self.compute_cartesian) # self.compute_cartesian() def grabcamera(self): # self.putcamerapose= {'left_s0': -np.pi/4, # 'left_s1': -np.pi/12, # 'left_e0': 0, # 'left_e1': 0, # 'left_w0': 0, # 'left_w1': np.pi/12, # 'left_w2': 0} # self.left_arm_group.set_joint_value_target(self.putcamerapose) # plan1=self.left_arm_group.plan() # self.left_arm_group.go() # rospy.loginfo("=======put camera in left gripper please=======") # raw_input() # self.left_gripper.close() # rospy.sleep(3.0) self.holdcamerapose = { 'left_s0': -0.04678641403050512, 'left_s1': -1.4001409641424114, 'left_e0': 0.3712233506682701, 'left_e1': 1.3940050409908697, 'left_w0': -0.46326219794139495, 'left_w1': 2.05706823655434, 'left_w2': -0.943014689352558 } self.left_arm_group.set_joint_value_target(self.holdcamerapose) plan2 = self.left_arm_group.plan() self.left_arm_group.go() rospy.sleep(2.0) def rangecallback(self, data): self.rangestatetemp = data self.rangestate = self.rangestatetemp.range def go_startpose(self): self.move = "0" self.moving_publisher.publish(self.move) global placetimes # self.moving='1' # self.moving_publisher(self.moving) #Natural position of baxter self.initialpose = { 'right_s0': 0, #0.03413107253045045, 'right_s1': -50 * np.pi / 180, #-0.6937428113211783, 'right_e0': 0, #0.1277039005914607, 'right_e1': 70 * np.pi / 180, #0.9671748867617533, 'right_w0': 0, #-0.16835439147042416, 'right_w1': 72 * np.pi / 180, #1.0810729602622453, 'right_w2': 0 } #-0.1472621556369997} self.right_arm_group.set_joint_value_target(self.initialpose) rospy.loginfo("Attempting to start pose") plan = self.right_arm_group.plan() self.right_arm_group.go() # rospy.sleep(1.0) # if placetimes>0: # rospy.loginfo("==========Ready to fetch another number, Press 'Enter' to cotinue=======") # raw_input() # self.standbypose() self.standbypose() def standbypose(self): global placetimes if placetimes < 3: self.placex_coord = place_areax[0] self.placey_coord = place_areay[placetimes] elif placetimes < 6: self.placex_coord = place_areax[1] self.placey_coord = place_areay[placetimes - 3] elif placetimes < 9: self.placex_coord = place_areax[2] self.placey_coord = place_areay[placetimes - 6] else: rospy.loginfo( "========There is currently no place to put block, Please press 'Enter' after cleaning the table." ) raw_input() placetimes = 0 self.placex_coord = place_areax[0] self.placey_coord = place_areay[placetimes] #self.compute_cartesian() def compute_cartesian(self, data): # self.moving='0' # self.moving_publisher(self.moving) # testingdata=np.array([1, 0.55597888,-0.24354399,-0.3,0]) # data=testingdata data = data.data.split('&') fetchnumber = float(data[0]) # rospy.loginfo("=============Trying to fetch number " + str(fetchnumber) + " Press 'Enter' to confirm==============") confirm = raw_input("=============Trying to fetch number " + str(fetchnumber) + " Press 'y/n' to confirm==============") # confirm ='y' if confirm == "y": rospy.loginfo("==============Received desired position") #extract x,y,z coordinate from subscriber data self.x_coord = float(data[1]) - 0.020 # rospy.loginfo(str(self.x_coord)) self.y_coord = float(data[2]) + 0.038 # rospy.loginfo(str(self.y_coord)) self.z_coord = -0.3 self.theta = 0 self.pose_target = converttoSE3(self.x_coord, self.y_coord, self.z_coord, self.theta) # try: self.move_arm_standoff() else: self.go_startpose() # except: # self.right_gripper.open() # self.go_startpose() def move_arm_standoff(self): pose_target = converttoSE3(self.x_coord, self.y_coord, self.z_coord + self.standoff, self.theta) theta0list = np.array([0.74, -0.67, 0.2, 1.2, 0, 1.068801113959162, 0]) joint_targettemp = IKinBody(self.Blist, self.M, pose_target, theta0list, 0.01, 0.001) if joint_targettemp[1] == True: rospy.loginfo( "Solution Found! Joint target value computed successfully.") print(joint_targettemp[0]) self.pickstandoff_joint_target = { 'right_s0': joint_targettemp[0][0], #+0.05, 'right_s1': joint_targettemp[0][1], 'right_e0': joint_targettemp[0][2], 'right_e1': joint_targettemp[0][3], 'right_w0': joint_targettemp[0][4], 'right_w1': joint_targettemp[0][5], 'right_w2': joint_targettemp[0][6] } # try: self.right_arm_group.set_joint_value_target( self.pickstandoff_joint_target) rospy.loginfo("Attempting to go to pick stand off position") plan = self.right_arm_group.plan() self.right_arm_group.go() rospy.sleep(1.0) self.move_arm_pick() # except: # self.right_gripper.open() # self.go_startpose() else: rospy.logerr( "Could not find valid joint target value, returnning to Initial position" ) self.right_gripper.open() self.go_startpose() def move_arm_pick(self): pose_target = converttoSE3(self.x_coord, self.y_coord, self.z_coord, self.theta) theta0list = np.array([ self.pickstandoff_joint_target['right_s0'], self.pickstandoff_joint_target['right_s1'], self.pickstandoff_joint_target['right_e0'], self.pickstandoff_joint_target['right_e1'], self.pickstandoff_joint_target['right_w0'], self.pickstandoff_joint_target['right_w1'], self.pickstandoff_joint_target['right_w2'] ]) joint_targettemp = IKinBody(self.Blist, self.M, pose_target, theta0list, 0.01, 0.001) if joint_targettemp[1] == True: rospy.loginfo( "Solution Found! Joint target value computed successfully.") print(joint_targettemp[0]) self.pick_joint_target = { 'right_s0': joint_targettemp[0][0], 'right_s1': joint_targettemp[0][1], 'right_e0': joint_targettemp[0][2], 'right_e1': joint_targettemp[0][3], 'right_w0': joint_targettemp[0][4], 'right_w1': joint_targettemp[0][5], 'right_w2': joint_targettemp[0][6] } self.right_arm_group.set_joint_value_target(self.pick_joint_target) rospy.loginfo("Attempting to go to pick position") plan = self.right_arm_group.plan() self.right_arm_group.go() rospy.sleep(1.0) print(self.rangestate) if self.rangestate < 0.2: self.right_gripper.close() else: self.right_gripper.open() self.move_arm_backstandoff() else: rospy.logerr( "Could not find valid joint target value, returnning to Initial position" ) self.right_gripper.open() self.go_startpose() # self.move_arm_backstandoff() def move_arm_backstandoff(self): theta0list = np.array([ self.pick_joint_target['right_s0'], self.pick_joint_target['right_s1'], self.pick_joint_target['right_e0'], self.pick_joint_target['right_e1'], self.pick_joint_target['right_w0'], self.pick_joint_target['right_w1'], self.pick_joint_target['right_w2'] ]) pose_target = converttoSE3(self.x_coord, self.y_coord, self.z_coord + self.standoff, self.theta) joint_targettemp = IKinBody(self.Blist, self.M, pose_target, theta0list, 0.01, 0.001) if joint_targettemp[1] == True: rospy.loginfo( "Solution Found! Joint target value computed successfully.") print(joint_targettemp[0]) self.pickbackstandoff_joint_target = { 'right_s0': joint_targettemp[0][0], 'right_s1': joint_targettemp[0][1], 'right_e0': joint_targettemp[0][2], 'right_e1': joint_targettemp[0][3], 'right_w0': joint_targettemp[0][4], 'right_w1': joint_targettemp[0][5], 'right_w2': joint_targettemp[0][6] } self.right_arm_group.set_joint_value_target( self.pickbackstandoff_joint_target) rospy.loginfo("Attempting to go back to pick standoff position") plan = self.right_arm_group.plan() self.right_arm_group.go() rospy.sleep(1.0) if self.rangestate < 0.3: self.right_gripper.close() self.move_arm_standoff2() else: rospy.loginfo( "=========Sorry, I drop the block, returning to start pose=====" ) self.right_gripper.open() self.go_startpose() else: rospy.logerr( "Could not find valid joint target value, returnning to Initial position" ) self.right_gripper.open() self.go_startpose() def move_arm_standoff2(self): theta0list = np.array([ self.pickbackstandoff_joint_target['right_s0'], self.pickbackstandoff_joint_target['right_s1'], self.pickbackstandoff_joint_target['right_e0'], self.pickbackstandoff_joint_target['right_e1'], self.pickbackstandoff_joint_target['right_w0'], self.pickbackstandoff_joint_target['right_w1'], self.pickbackstandoff_joint_target['right_w2'] ]) # theta0list=np.array([0.8,-0.64,0.25,1.0400389741863105,0.2,1.068801113959162,0]) pose_target = converttoSE3(self.placex_coord, self.placey_coord, self.z_coord + self.standoff, self.theta) joint_targettemp = IKinBody(self.Blist, self.M, pose_target, theta0list, 0.01, 0.001) if joint_targettemp[1] == True: rospy.loginfo( "Solution Found! Joint target value computed successfully.") print(joint_targettemp[0]) self.placestandoff_joint_target = { 'right_s0': joint_targettemp[0][0], 'right_s1': joint_targettemp[0][1], 'right_e0': joint_targettemp[0][2], 'right_e1': joint_targettemp[0][3], 'right_w0': joint_targettemp[0][4], 'right_w1': joint_targettemp[0][5], 'right_w2': joint_targettemp[0][6] } self.right_arm_group.set_joint_value_target( self.placestandoff_joint_target) rospy.loginfo("Attempting to go to place stand off position") plan = self.right_arm_group.plan() self.right_arm_group.go() rospy.sleep(1.0) if self.rangestate < 0.3: self.right_gripper.close() self.move_arm_place() else: rospy.loginfo("Sorry, I drop the block, return to start pose") self.right_gripper.open() self.go_startpose() else: rospy.logerr( "Could not find valid joint target value, returnning to Initial position" ) self.right_gripper.open() self.go_startpose() # rospy.sleep(1.0) def move_arm_place(self): global placetimes pose_target = converttoSE3(self.placex_coord, self.placey_coord, self.z_coord, 0) theta0list = np.array([ self.placestandoff_joint_target['right_s0'], self.placestandoff_joint_target['right_s1'], self.placestandoff_joint_target['right_e0'], self.placestandoff_joint_target['right_e1'], self.placestandoff_joint_target['right_w0'], self.placestandoff_joint_target['right_w1'], self.placestandoff_joint_target['right_w2'] ]) joint_targettemp = IKinBody(self.Blist, self.M, pose_target, theta0list, 0.01, 0.001) if joint_targettemp[1] == True: rospy.loginfo( "Solution Found! Joint target value computed successfully.") print(joint_targettemp[0]) self.place_joint_target = { 'right_s0': joint_targettemp[0][0], 'right_s1': joint_targettemp[0][1], 'right_e0': joint_targettemp[0][2], 'right_e1': joint_targettemp[0][3], 'right_w0': joint_targettemp[0][4], 'right_w1': joint_targettemp[0][5], 'right_w2': joint_targettemp[0][6] } self.right_arm_group.set_joint_value_target( self.place_joint_target) rospy.loginfo("Attempting to go to pick position") plan = self.right_arm_group.plan() self.right_arm_group.go() placetimes = placetimes + 1 self.right_gripper.open() # print(placetimes) self.move_arm_backstandoff2() else: rospy.logerr("Could not find valid joint target value") self.go_startpose() def move_arm_backstandoff2(self): theta0list = np.array([ self.place_joint_target['right_s0'], self.place_joint_target['right_s1'], self.place_joint_target['right_e0'], self.place_joint_target['right_e1'], self.place_joint_target['right_w0'], self.place_joint_target['right_w1'], self.place_joint_target['right_w2'] ]) pose_target = converttoSE3(self.placex_coord, self.placey_coord, self.z_coord + self.standoff, 0) # theta0list=np.array([0.74,-0.585980660972228,0,1.0400389741863105,0,1.068801113959162,0]) joint_targettemp = IKinBody(self.Blist, self.M, pose_target, theta0list, 0.01, 0.001) if joint_targettemp[1] == True: rospy.loginfo( "Solution Found! Joint target value computed successfully.") print(joint_targettemp[0]) self.placebackstandoff_joint_target = { 'right_s0': joint_targettemp[0][0], 'right_s1': joint_targettemp[0][1], 'right_e0': joint_targettemp[0][2], 'right_e1': joint_targettemp[0][3], 'right_w0': joint_targettemp[0][4], 'right_w1': joint_targettemp[0][5], 'right_w2': joint_targettemp[0][6] } self.right_arm_group.set_joint_value_target( self.placebackstandoff_joint_target) rospy.loginfo("Attempting to go to place stand off position") plan = self.right_arm_group.plan() self.right_arm_group.go() self.right_gripper.open() self.go_startpose() else: rospy.logerr("Could not find valid joint target value") self.go_startpose()
class Baxterpicknumber(): def __init__(self): rospy.loginfo("++++++++++++Ready to move the robot+++++++++++") #Initialize moveit_commander self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm") #Display Trajectory in RVIZ self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.moving_publisher = rospy.Publisher('amimoving', String, queue_size=10) rospy.sleep(3.0) # print self.robot.get_current_state() # print "" self.planning_frame = self.right_arm_group.get_planning_frame() rospy.loginfo("============ Reference frame: %s" % self.planning_frame) self.eef_link = self.right_arm_group.get_end_effector_link() print "============ End effector: %s" % self.eef_link self.group_names = self.robot.get_group_names() print "============ Robot Groups:", self.robot.get_group_names() #set planning and execution parameters self.right_arm_group.set_goal_position_tolerance(0.01) self.right_arm_group.set_goal_orientation_tolerance(0.01) self.right_arm_group.set_planning_time(5.0) self.right_arm_group.allow_replanning(False) self.right_arm_group.set_max_velocity_scaling_factor(0.6) self.right_arm_group.set_max_acceleration_scaling_factor(0.6) #specify which gripper and limb are taking command self.right_gripper = Gripper('right', CHECK_VERSION) self.right_gripper.reboot() self.right_gripper.calibrate() # self.limb = baxter_interface.Limb('right') # self.angles = self.limb.joint_angles() # self.wave_1 = {'right_s0': -0.459, 'right_s1': -0.202, 'right_e0': 1.807, 'right_e1': 1.714, 'right_w0': -0.906, 'right_w1': -1.545, 'right_w2': -0.276} # rospy.sleep(3) # self.limb.move_to_joint_positions(self.wave_1) self.pose_target = Pose() self.standoff = 0.1 self.__right_sensor = rospy.Subscriber( '/robot/range/right_hand_range/state', Range, self.rangecallback) self.rangestatetemp = Range() self.box_name = 'table' self.moving = '0' self.moving_publisher.publish(self.moving) def add_box(self, timeout=4): self.box_pose = PoseStamped() self.box_pose.header.frame_id = "base" self.box_pose.pose.position.x = 1.0 self.box_pose.pose.position.z = -0.27 self.box_pose.pose.orientation.w = 1.0 self.scene.add_box(self.box_name, self.box_pose, size=(1.5, 2, 0.1)) # return self.wait_for_state_update(box_is_known=True, timeout=timeout) def wait_for_state_update(self, box_is_known=False, box_is_attached=False, timeout=4): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. box_name = self.box_name scene = self.scene ## BEGIN_SUB_TUTORIAL wait_for_scene_update ## ## Ensuring Collision Updates Are Receieved ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## If the Python node dies before publishing a collision object update message, the message ## could get lost and the box will not appear. To ensure that the updates are ## made, we wait until we see the changes reflected in the ## ``get_known_object_names()`` and ``get_known_object_names()`` lists. ## For the purpose of this tutorial, we call this function after adding, ## removing, attaching or detaching an object in the planning scene. We then wait ## until the updates have been made or ``timeout`` seconds have passed start = rospy.get_time() seconds = rospy.get_time() while (seconds - start < timeout) and not rospy.is_shutdown(): # Test if the box is in attached objects attached_objects = scene.get_attached_objects([box_name]) is_attached = len(attached_objects.keys()) > 0 # Test if the box is in the scene. # Note that attaching the box will remove it from known_objects is_known = box_name in scene.get_known_object_names() # Test if we are in the expected state if (box_is_attached == is_attached) and (box_is_known == is_known): return True # Sleep so that we give other threads time on the processor rospy.sleep(0.1) seconds = rospy.get_time() # If we exited the while loop without returning then we timed out return False def rangecallback(self, data): self.rangestatetemp = data self.rangestate = self.rangestatetemp.range def compute_cartesian(self): #,data): testingdata = np.array([0.6, -0.320147457674, -0.209, np.pi]) data = testingdata rospy.loginfo("++++++++++Received desired position+++++++++") #extract x,y,z coordinate from subscriber data self.x_coord = data[0] self.y_coord = data[1] self.z_coord = data[2] theta = data[3] #put coordinate data back to arrays pout = np.array([self.x_coord, self.y_coord, self.z_coord]) block_orientation = tr.quaternion_from_euler(theta, 0, 0, 'sxyz') #convert orentation and position to Quaternion quat = Quaternion(*block_orientation) self.quat_position = Point(*pout) self.quat_orientation = copy.deepcopy(quat) self.cartesian_reader = True # def move_arm_standoff_cartesianpath(self): # self.moving=True # waypoints=[] # rospy.sleep(1.0) # wpose=self.pose_target # wpose.posistion.z= self.pose_target # wpose.position=self.quat_position # waypoints.append(copy.deepcopy(wpose)) # print('moving to standoff') # (plan,fraction)=self.right_arm_group.compute_cartesian_path(waypoints,0.01,0.0) # self.right_arm_group.execute(plan,wait=True) def move_arm_standoff(self): self.moving = '1' self.moving_publisher.publish(self.moving) rospy.loginfo("+++++++++++++Going to standoff posistion+++++++++") #def standoff height (above the block) #standoff position has same gripper orentation as block orientation self.pose_target.orientation = self.quat_orientation #add standoff height to the height of the block and get new pose_target.position z_standoff = self.z_coord + self.standoff pout = np.array([self.x_coord, self.y_coord, z_standoff]) self.pose_target.position = Point(*pout) #Calling Moveit to use IK to compute the plan and execute it print(self.pose_target) self.right_arm_group.set_pose_target(self.pose_target) rospy.sleep(3.0) plan_standoff = self.right_arm_group.go(wait=True) self.right_arm_group.stop() self.right_arm_group.clear_pose_targets() rospy.sleep(1.0) # def move_arm_pick_cartesianpath(self,scale=1): # waypoints=[] # rospy.sleep(1.0) # wpose=self.pose_target # wpose.position=self.quat_position # waypoints.append(copy.deepcopy(wpose)) # print('moving to pick up') # (plan,fraction)=self.right_arm_group.compute_cartesian_path(waypoints,0.01,0.0) # self.right_arm_group.execute(plan,wait=True) def remove_box(self): self.scene.remove_world_object(self.box_name) def move_arm_pick(self): rospy.loginfo("+++++++++++++Going to pickup posistion+++++++++") #standoff position has same gripper orentation and height as block orientation self.pose_target.orientation = self.quat_orientation z_standoff = self.z_coord pout = np.array([self.x_coord, self.y_coord, z_standoff]) self.pose_target.position = Point(*pout) #Calling Moveit to use IK to compute the plan and execute it print(self.pose_target) self.right_arm_group.set_pose_target(self.pose_target) rospy.sleep(2.0) plan_standoff = self.right_arm_group.go(wait=True) print(self.rangestate) if self.rangestate < 0.4: self.right_gripper.close() else: self.right_gripper.open() self.right_arm_group.stop() self.right_arm_group.clear_pose_targets() rospy.sleep(1.0) # def move_arm_back_standoff_cartesianpath(self,scale=1): # waypoints=[] # rospy.sleep(1.0) # wpose=self.pose_target # waypoints.append(copy.deepcopy(wpose)) # (plan,fraction)=self.right_arm_group.compute_cartesian_path(waypoints,0.005,0.01) # self.right_arm_group.execute(plan,wait=True) def move_arm_back_standoff(self): rospy.loginfo( "+++++++++++++Going back to stand off posistion+++++++++") self.right_gripper.close() #def standoff height (above the block) #standoff position has same gripper orentation as block orientation self.pose_target.orientation = self.quat_orientation #add standoff height to the height of the block and get new pose_target.position z_standoff = self.z_coord + self.standoff pout = np.array([self.x_coord, self.y_coord, z_standoff]) self.pose_target.position = Point(*pout) #Calling Moveit to use IK to compute the plan and execute it print(self.pose_target) self.right_arm_group.set_pose_target(self.pose_target) rospy.sleep(2.0) plan_backtostandoff = self.right_arm_group.go(wait=True) self.right_arm_group.stop() self.right_arm_group.clear_pose_targets() rospy.sleep(1.0) # def move_arm_standoff2_cartesianpath(self,scale=1): # self.pose_target.position=self.quat_position # waypoints=[] # rospy.sleep(1.0) # wpose=self.pose_target # wpose.position.z = scale * self.standoff # waypoints.append(copy.deepcopy(wpose)) # (plan,fraction)=self.right_arm_group.compute_cartesian_path(waypoints,0.01,0.0) # self.right_arm_group.execute(plan,wait=True) def move_arm_standoff2(self): rospy.loginfo("+++++++++++++Going to standoff2 posistion+++++++++") #def standoff height (above the block) self.right_gripper.close() #standoff position has same gripper orentation as block orientation self.pose_target.orientation = self.quat_orientation #add standoff height to the height of the block and get new pose_target.position x_standoff2 = self.x_coord z_standoff2 = self.z_coord + self.standoff y_standoff2 = self.y_coord + 0.2 pout = np.array([x_standoff2, y_standoff2, z_standoff2]) self.pose_target.position = Point(*pout) #Calling Moveit to use IK to compute the plan and execute it print(self.pose_target) self.right_arm_group.set_pose_target(self.pose_target) rospy.sleep(3.0) plan_standoff2 = self.right_arm_group.go(wait=True) self.right_arm_group.stop() self.right_arm_group.clear_pose_targets() rospy.sleep(1.0) # def move_arm_place_cartesianpath(self,scale=1): # waypoints=[] # rospy.sleep(1.0) # wpose=self.pose_target # wpose.position.z = self.quat_position.z # waypoints.append(copy.deepcopy(wpose)) # (plan,fraction)=self.right_arm_group.compute_cartesian_path(waypoints,0.005,0.01) # self.right_arm_group.execute(plan,wait=True) def move_arm_place(self): global placedtimes rospy.loginfo("+++++++++++++Going to place posistion+++++++++") self.right_gripper.close() #standoff position has same gripper orentation and height as block orientation self.pose_target.orientation = self.quat_orientation # z_standoff=block_height/2 in case need gripper half height higer than predicted block height. self.pose_target.position = self.quat_position self.placetarget_y = np.linspace(0.2, 0.29, 10) x_place = self.x_coord z_place = self.z_coord y_place = self.y_coord + self.placetarget_y[placedtimes] pout = np.array([x_place, y_place, z_place]) self.pose_target.position = Point(*pout) #Calling Moveit to use IK to compute the plan and execute it print(self.pose_target) self.right_arm_group.set_pose_target(self.pose_target) rospy.sleep(2.0) plan_place = self.right_arm_group.go(wait=True) self.right_gripper.open() self.right_arm_group.stop() self.right_arm_group.clear_pose_targets() rospy.sleep(1.0) def move_arm_backstandoff2(self): global placedtimes rospy.loginfo( "+++++++++++++Going back to standoff2 posistion+++++++++") #def standoff height (above the block) self.right_gripper.open() #standoff position has same gripper orentation as block orientation self.pose_target.orientation = self.quat_orientation #add standoff height to the height of the block and get new pose_target.position x_standoff2 = self.x_coord z_standoff2 = self.z_coord + self.standoff y_standoff2 = self.y_coord + self.placetarget_y[placedtimes] pout = np.array([x_standoff2, y_standoff2, z_standoff2]) self.pose_target.position = Point(*pout) #Calling Moveit to use IK to compute the plan and execute it print(self.pose_target) self.right_arm_group.set_pose_target(self.pose_target) rospy.sleep(2.0) plan_backstandoff2 = self.right_arm_group.go(wait=True) self.right_arm_group.stop() self.right_arm_group.clear_pose_targets() rospy.sleep(1.0) placedtimes += 1 self.moving = '0' self.moving_publisher.publish(self.moving)