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)
