class teleop(object):
def __init__(self):
self.lastpose = Pose()
self.lasttime = time.time()
self.need_plan = True
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
def callback_rosbridge(self, data):
if data.pose != self.lastpose:
self.lastpose = data.pose
self.lasttime = time.time()
self.need_plan = True
def check_pose_updates(self):
if (not (self.need_plan and time.time() - self.lasttime > move_delay)):
return
self.need_plan = False
pose_goal = self.lastpose
gripper_frame = 'wrist_roll_link'
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
gripper_pose_stamped.header.stamp = rospy.Time.now()
gripper_pose_stamped.pose = pose_goal
self.move_group.moveToPose(gripper_pose_stamped, gripper_frame)
print('Moved To New Pose')
def main(args):
rospy.init_node('simple_move')
move_group = MoveGroupInterface('manipulator', "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
'shoulder_1_joint', 'shoulder_2_joint', 'elbow_1_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
# This is the wrist link not the gripper itself
gripper_frame = "wrist_3_link"
poses, pose_name = get_pose(args.reset)
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = "base_link"
while not rospy.is_shutdown():
for pose in poses:
# Finish building the Pose_stamped message
# If the message stampe is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
gripper_pose_stamped.pose = pose
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
log_output = pose_name + " Done!"
rospy.loginfo(log_output)
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
class Arm:
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6)
def MoveToPose(self, X, Y, Z, Roll, Pitch, Yaw):
orientation = tf.transformations.quaternion_from_euler(
Roll, Pitch, Yaw)
self.poseStamped = PoseStamped()
self.poseStamped.header.frame_id = 'base_link'
self.poseStamped.header.stamp = rospy.Time.now()
self.poseStamped.pose.position.x = X
self.poseStamped.pose.position.y = Y
self.poseStamped.pose.position.z = Z
self.poseStamped.pose.orientation.x = orientation[0]
self.poseStamped.pose.orientation.y = orientation[1]
self.poseStamped.pose.orientation.z = orientation[2]
self.poseStamped.pose.orientation.w = orientation[3]
self.moveGroup.moveToPose(self.poseStamped,
'gripper_link',
0.005,
max_velocity_scaling_factor=0.2)
self.result = self.moveGroup.get_move_action().get_result()
def Tuck(self):
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
self.moveGroup.moveToJointPosition(joints,
pose,
0.01,
max_velocity_scaling_factor=0.2)
while not rospy.is_shutdown():
self.result = self.moveGroup.get_move_action().get_result()
if self.result.error_code.val == MoveItErrorCodes.SUCCESS:
break
def AddDice(self, name, X, Y, Z):
self.planning_scene.addCube(name, 0.1, X, Y, Z)
def RemoveDice(self, name):
self.planning_scene.removeCollisionObject(name)
class TrajectorySimulator():
def __init__(self):
self.group = MoveGroupInterface("arm", "base_link")
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(15)
print "============ Starting planning"
def plan_to_pose_goal(self, goal):
# Extract goal position
pose_x = goal[0]
pose_y = goal[1]
pose_z = goal[2]
# Extract goal orientation--provided in Euler angles
roll = goal[3]
pitch = goal[4]
yaw = goal[5]
# Convert Euler angles to quaternion, create PoseStamped message
quaternion = quaternion_from_euler(roll, pitch, yaw)
pose_target = PoseStamped()
pose_target.pose.position.x = pose_x
pose_target.pose.position.y = pose_y
pose_target.pose.position.z = pose_z
pose_target.pose.orientation.x = quaternion[0]
pose_target.pose.orientation.y = quaternion[1]
pose_target.pose.orientation.z = quaternion[2]
pose_target.pose.orientation.w = quaternion[3]
pose_target.header.frame_id = "base_link"
pose_target.header.stamp = rospy.Time.now()
print "============= PoseStamped message filled out"
# Execute motion
while not rospy.is_shutdown():
result = self.group.moveToPose(pose_target,
"gripper_link",
tolerance=0.3,
plan_only=True)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "================ Pose Achieved"
return result.planned_trajectory
def plan_to_jointspace_goal(self, pose):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
while not rospy.is_shutdown():
result = self.group.moveToJointPosition(joints, pose)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "============ Pose Achieved"
return "Success"
class RobotArm:
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.removeCollisionObject("table1")
def MoveToPose(self,X,Y,Z,x,y,z,w):
self.poseStamped = PoseStamped()
self.poseStamped.header.frame_id = 'base_link'
self.poseStamped.header.stamp = rospy.Time.now()
self.poseStamped.pose.position.x = X
self.poseStamped.pose.position.y = Y
self.poseStamped.pose.position.z = Z
self.poseStamped.pose.orientation.x = x
self.poseStamped.pose.orientation.y = y
self.poseStamped.pose.orientation.z = z
self.poseStamped.pose.orientation.w = w
self.moveGroup.moveToPose(self.poseStamped, 'gripper_link')
self.result = self.moveGroup.get_move_action().get_result()
def OriginReturn(self):
#Move to the origin
joints = ["torso_lift_joint","shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
self.moveGroup.moveToJointPosition(joints, pose, 0.02)
while 1:
self.result = self.moveGroup.get_move_action().get_result()
if self.result.error_code.val == MoveItErrorCodes.SUCCESS:
break
class TrajectorySimulator():
def __init__(self):
self.group = MoveGroupInterface("arm", "base_link")
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(15)
print "============ Starting planning"
def plan_to_pose_goal(self, goal):
# Extract goal position
pose_x = goal[0]
pose_y = goal[1]
pose_z = goal[2]
# Extract goal orientation--provided in Euler angles
roll = goal[3]
pitch = goal[4]
yaw = goal[5]
# Convert Euler angles to quaternion, create PoseStamped message
quaternion = quaternion_from_euler(roll, pitch, yaw)
pose_target = PoseStamped()
pose_target.pose.position.x = pose_x
pose_target.pose.position.y = pose_y
pose_target.pose.position.z = pose_z
pose_target.pose.orientation.x = quaternion[0]
pose_target.pose.orientation.y = quaternion[1]
pose_target.pose.orientation.z = quaternion[2]
pose_target.pose.orientation.w = quaternion[3]
pose_target.header.frame_id = "base_link"
pose_target.header.stamp = rospy.Time.now()
print "============= PoseStamped message filled out"
# Execute motion
while not rospy.is_shutdown():
result = self.group.moveToPose(pose_target, "gripper_link",
tolerance = 0.3, plan_only=True)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "================ Pose Achieved"
return result.planned_trajectory
def plan_to_jointspace_goal(self, pose):
joints = ["shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint",
"forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"]
while not rospy.is_shutdown():
result = self.group.moveToJointPosition(joints, pose)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "============ Pose Achieved"
return "Success"
class Sensory_data_collection:
def __init__(self, object_name, trialnum):
rospy.init_node('pnp', anonymous=True)
rospy.Subscriber('/joy', Joy, self.joycallback)
print('starting...')
self.calibrated = False
self.both_arms_group = MoveGroupInterface("both_arms", "base")
self.right_arm_group = MoveGroupInterface("right_arm", "base")
self.left_arm_group = MoveGroupInterface("left_arm", "base")
self.leftgripper = baxter_interface.Gripper('left')
self.rightgripper = baxter_interface.Gripper('right')
self.leftgripper.calibrate()
self.leftgripper.open()
self.left_arm = baxter_interface.Limb('left')
self.right_arm = baxter_interface.Limb('right')
self.rightgripper.calibrate()
self.rightgripper.open()
self.both_arm_joints = [
'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0',
'left_s1', 'right_s0', 'right_s1', 'right_w0', 'right_w1',
'right_w2', 'right_e0', 'right_e1'
]
self.right_joints = [
'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2',
'right_e0', 'right_e1'
]
self.left_joints = [
'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0',
'left_s1'
]
self.init_joint_angles = [
0.2515728492132078, 1.58958759144626, 3.0418839023767754,
-1.2862428906419194, 1.2916118233995184, 0.37735927381981177,
-1.3203739631723699, -0.1514806028036846, -1.2908448330055757,
2.908044078633773, -1.5424176822187836, -0.34131072530450457,
0.984048675428493, 1.1949710337627373
]
self.preright = [
0.27381557063754636, -0.7309418454273996, 2.269908070873441,
-1.0181797479589434, 0.9177040063524488, 1.1213399559442374,
1.878359474765689
]
self.preleft = [
0.7294078646395142, 1.1850001586414822, 1.957359485341788,
-0.9821311994436361, 1.5397332158399841, -0.7742768026851626,
-0.7566360236244803
]
self.prepush = [
0.6914418401393503, 0.877820505867428, 0.5725583290782307,
-0.7827136970185323, 1.7636944108712544, -0.6354515413815326,
-0.7646894227608787
]
self.postpush1 = [
0.5767767762449155, 1.0749370371107037, 0.5591359971842333,
-0.68568941218478, 1.4166312576121796, -0.9744612955042091,
-0.6653641667452982
]
self.postpush2 = [
0.5556845404114912, 1.135912773429149, 0.4275971446230591,
-0.656543777214957, 1.0020729496861465, -1.23255356306593,
-0.4686311306989939
]
self.shake1 = [
0.7232719414879726, 1.1455001533534328, 0.9177040063524488,
-0.9652574107768966, 1.551621566946096, -0.7735098122912198,
-0.7631554419729933
]
self.shake2 = [
0.6753350418665534, 1.016645767171058, 2.992413021967471,
-0.9269078910797612, 1.6712720684011584, -0.8011214664731573,
-0.8084078752156131
]
self.shake3 = [
0.6412039693361029, 0.34284470609239, 2.865476111769953,
-1.007825377640717, 1.3219079439602552, -0.7554855380335662,
-0.8199127311247536
]
self.shake4 = [
1.4580487388850858, 1.326893381520883, 1.8634031620838063,
-0.9882671225951778, 1.2517283229144975, -0.6830049458059805,
-0.9782962474739226
]
self.prepoke = [
0.6170437719269076, 0.8969952657159956, 2.0129662889026343,
-0.9119515783978784, 1.9036701577657984, -0.5215534678810406,
-0.9560535260495842
]
self.pickgoal = PoseStamped()
self.pickgoal.header.frame_id = "base"
self.pickgoal.header.stamp = rospy.Time.now()
self.pickgoal.pose.position.x = 0.57
self.pickgoal.pose.position.y = -0.1
self.pickgoal.pose.position.z = -0.05
self.pickgoal.pose.orientation.x = 1.0
self.pickgoal.pose.orientation.y = 0.0
self.pickgoal.pose.orientation.z = 0.0
self.pickgoal.pose.orientation.w = 0.0
#data saving attributes
self.current_image = None
self.bridge = CvBridge()
self.current_jointstate = {}
self.current_endpointstate = {}
self.current_accelerometer = {}
self.keep_saving = False
self.accel_counter = 0
self.joint_counter = 0
self.endpoint_counter = 0
self.pause_event = Event()
rospy.Subscriber('/robot/accelerometer/left_accelerometer/state', Imu,
self.accel_callback)
rospy.Subscriber('/robot/limb/left/endpoint_state', EndpointState,
self.endpoint_callback)
rospy.Subscriber('/robot/joint_states', JointState,
self.joint_callback)
rospy.Subscriber('/camera/rgb/image_raw', Image, self.image_callback)
self.object_name = object_name
self.trialnum = trialnum
if not os.path.exists(os.path.dirname(self.object_name)):
try:
os.mkdir(self.object_name)
except OSError as exc:
pass
self.q = queue.Queue()
self.should_record = False
self.both_arms_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
rospy.sleep(5)
self.perform_action_sequence('left')
rospy.spin()
def perform_action_sequence(self, arm):
self.capture_image(self.trialnum, self.object_name)
self.lift(arm)
self.shake(arm)
self.drop(arm)
self.push(arm)
rospy.sleep(6)
self.poke(arm)
self.both_arms_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
def joycallback(self, data):
if data.buttons[3] == 1: #X
arm = 'left'
self.keep_saving = True
self.capture_image('lift', self.object_name)
self.start_recording_audio('lift', self.object_name)
self.lift(arm)
self.stop_recording_audio()
self.save_data('lift', self.object_name)
elif data.buttons[1] == 1: #B
arm = 'left'
self.keep_saving = True
self.capture_image('push', self.object_name)
self.start_recording_audio('push', self.object_name)
self.push(arm)
self.stop_recording_audio()
self.save_data('push', self.object_name)
elif data.buttons[4] == 1: #Y
arm = 'left'
self.keep_saving = True
self.capture_image('drop', self.object_name)
self.start_recording_audio('drop', self.object_name)
self.drop(arm)
self.stop_recording_audio()
self.save_data('drop', self.object_name)
elif data.buttons[0] == 1: #A
arm = 'left'
self.keep_saving = True
self.capture_image('shake', self.object_name)
self.start_recording_audio('shake', self.object_name)
self.shake(arm)
self.stop_recording_audio()
self.save_data('shake', self.object_name)
def record_callback(self, indata, frames, time, status):
self.q.put(indata.copy())
def start_recording_audio(self, action, object_name):
self.should_record = True
thread.start_new_thread(self.record_audio, (action, object_name))
def stop_recording_audio(self):
self.should_record = False
def record_audio(self, action, object_name):
try:
name = object_name + '/baxter_' + action + "_audio_" + object_name + '_' + self.trialnum + '.wav'
with sf.SoundFile(name, mode='x', samplerate=48000,
channels=1) as file:
with sd.InputStream(samplerate=48000,
channels=1,
callback=self.record_callback):
print('#' * 80)
print('recording audio...')
print('#' * 80)
while self.should_record:
file.write(self.q.get())
self.q = queue.Queue()
except KeyboardInterrupt:
print('\nRecording finished: ' + repr('test.wav'))
parser.exit(0)
except Exception as e:
print(e)
def push(self, arm):
# Move both arms to start state
# self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False)
if arm == 'left':
self.left_arm_group.moveToJointPosition(self.left_joints,
self.prepush,
plan_only=False)
rospy.sleep(2)
self.keep_saving = True
self.start_recording_audio('push', self.object_name)
self.left_arm_group.moveToJointPosition(self.left_joints,
self.postpush1,
plan_only=False)
self.left_arm_group.moveToJointPosition(self.left_joints,
self.postpush2,
plan_only=False)
rospy.sleep(2)
self.stop_recording_audio()
self.save_data('push', self.object_name)
self.both_arms_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
rospy.sleep(5)
def shake(self, arm):
if arm == 'left':
#lift
#shake
self.keep_saving = True
self.start_recording_audio('shake', self.object_name)
self.left_arm_group.moveToJointPosition(self.left_joints,
self.shake1,
plan_only=False)
self.left_arm_group.moveToJointPosition(self.left_joints,
self.shake2,
plan_only=False)
self.left_arm_group.moveToJointPosition(self.left_joints,
self.shake3,
plan_only=False)
self.left_arm_group.moveToJointPosition(self.left_joints,
self.preleft,
plan_only=False)
self.left_arm_group.moveToJointPosition(self.left_joints,
self.shake4,
plan_only=False)
self.left_arm_group.moveToJointPosition(self.left_joints,
self.preleft,
plan_only=False)
self.stop_recording_audio()
self.save_data('shake', self.object_name)
rospy.sleep(5)
def drop(self, arm):
self.keep_saving = True
self.start_recording_audio('drop', self.object_name)
self.leftgripper.open()
rospy.sleep(5)
self.stop_recording_audio()
self.save_data('drop', self.object_name)
# rospy.sleep(5)
def poke(self, arm):
self.leftgripper.close()
self.left_arm_group.moveToJointPosition(self.left_joints,
self.prepoke,
plan_only=False)
self.keep_saving = True
self.start_recording_audio('poke', self.object_name)
self.left_arm_group.moveToJointPosition(self.left_joints,
self.preleft,
plan_only=False)
rospy.sleep(2)
self.stop_recording_audio()
self.save_data('poke', self.object_name)
rospy.sleep(5)
def lift(self, arm):
# Move both arms to start state
# self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False)
#pick
if arm == 'left':
self.left_arm_group.moveToJointPosition(self.left_joints,
self.preleft,
plan_only=False)
rospy.sleep(1)
self.keep_saving = True
self.start_recording_audio('lift', self.object_name)
self.left_arm_group.moveToPose(self.pickgoal,
"left_gripper",
plan_only=False)
self.leftgripper.close()
self.left_arm_group.moveToJointPosition(self.left_joints,
self.preleft,
plan_only=False)
self.stop_recording_audio()
self.save_data('lift', self.object_name)
else:
self.right_arm_group.moveToJointPosition(self.right_joints,
self.preright,
plan_only=False)
rospy.sleep(1)
self.right_arm_group.moveToPose(self.pickgoal,
"right_gripper",
plan_only=False)
self.rightgripper.close()
rospy.sleep(5)
#data saving methods
def accel_callback(self, data):
if self.keep_saving:
self.current_accelerometer[
self.accel_counter] = mc.convert_ros_message_to_dictionary(
data)
self.accel_counter += 1
def endpoint_callback(self, data):
if self.keep_saving:
self.current_endpointstate[
self.endpoint_counter] = mc.convert_ros_message_to_dictionary(
data)
self.endpoint_counter += 1
def joint_callback(self, data):
if self.keep_saving:
self.current_jointstate[
self.joint_counter] = mc.convert_ros_message_to_dictionary(
data)
self.joint_counter += 1
def image_callback(self, image):
self.current_image = image
def capture_image(self, action, object_name):
img = self.bridge.imgmsg_to_cv2(self.current_image, 'bgr8')
shape = img.shape
img = img[130:shape[0] - 230, 330:shape[1] - 200] #y,x
name = object_name + "/baxter_image_" + object_name + '_' + self.trialnum + '.png'
if cv2.imwrite(name, img):
print('Image saved')
else:
print('Could not save image')
def save_data(self, action, object_name):
self.keep_saving = False
accel_name = object_name + '/baxter_' + action + "_accel_" + object_name + '_' + self.trialnum + '.json'
endpoint_name = object_name + '/baxter_' + action + "_endstate_" + object_name + '_' + self.trialnum + '.json'
joint_name = object_name + '/baxter_' + action + "_jointstate_" + object_name + '_' + self.trialnum + '.json'
rospy.sleep(1)
with open(accel_name, "w") as accel_file:
d = copy.deepcopy(self.current_accelerometer)
json.dump(d, accel_file, indent=4)
self.current_accelerometer = {}
self.accel_counter = 0
with open(endpoint_name, "w") as endpoint_file:
e = copy.deepcopy(self.current_endpointstate)
json.dump(e, endpoint_file, indent=4)
self.current_endpointstate = {}
self.endpoint_counter = 0
with open(joint_name, "w") as joint_file:
j = copy.deepcopy(self.current_jointstate)
json.dump(j, joint_file, indent=4)
self.current_jointstate = {}
self.joint_counter = 0
print('Data saved')
def picknplace():
# Define initial parameters
p = PlanningSceneInterface("base")
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# Clear planning scene
p.clear()
# Add table as attached object
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# Move both arms to start state
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Loop to continue pick and place until all objects are cleared from table
j=0
k=0
while locs_x:
p.clear() # CLear planning scene of all collision objects
# Attach table as attached collision object to base of baxter
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
xn = locs_x[0]
yn = locs_y[0]
zn = -0.06
thn = orien[0]
sz = size[0]
if thn > pi/4:
thn = -1*(thn%(pi/4))
# Add collision objects into planning scene
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
for i in range(1,len(locs_x)):
p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
p.waitForSync()
# Move both arms to pos2 (Right arm away and left arm on table)
g.moveToJointPosition(jts_both, pos2, plan_only=False)
# Move left arm to pick object and pick object
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn+0.1
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles
pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
pickgoal.pose.position.z = zn
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
leftgripper.close()
pickgoal.pose.position.z = zn+0.1
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Move both arms to pos1
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Place object
stleft = PoseStamped()
stleft.header.frame_id = "base"
stleft.header.stamp = rospy.Time.now()
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.55
stleft.pose.position.z = 0.1
stleft.pose.orientation.x = 1.0
stleft.pose.orientation.y = 0.0
stleft.pose.orientation.z = 0.0
stleft.pose.orientation.w = 0.0
# Stack objects (large cubes) if size if greater than some threshold
if sz > 16.:
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.7
stleft.pose.position.z = -0.04+(k*0.05)
k += 1
gl.moveToPose(stleft, "left_gripper", plan_only=False)
leftgripper.open()
stleft.pose.position.z = 0.3
gl.moveToPose(stleft, "left_gripper", plan_only=False)
class TrajectoryGenerator():
# Wrapper class that facilitates trajectory planning.
# Includes methods for planning to arbitrary joint-space goals
# and end-effector pose goals.
def __init__(self):
self.group = MoveGroupInterface("arm", "base_link")
self.gripper = MoveGroupInterface("gripper", "base_link")
self.virtual_state = RobotState()
self.trajectory = None
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(5)
print "============ Starting planning"
def generate_trajectory(self, goal_poses):
for goal_pose in goal_poses:
if goal_pose.goal_type is "ee_pose":
traj_result = self.plan_to_ee_pose_goal(goal_pose.pose)
elif goal_pose.goal_type is "jointspace":
traj_result = self.plan_to_jointspace_goal(goal_pose.state)
elif goal_pose.goal_type is "gripper_posture":
traj_result = self.plan_to_gripper_goal(goal_pose.state)
else:
rospy.loginfo("Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'")
sys.exit()
approved_trajectories.append(traj_result)
return approved_trajectories
def plan_to_ee_pose_goal(self, goal):
# Extract goal position
pose_x = goal[0]
pose_y = goal[1]
pose_z = goal[2]
# Extract goal orientation--provided in Euler angles
roll = goal[3]
pitch = goal[4]
yaw = goal[5]
# Convert Euler angles to quaternion, create PoseStamped message
quaternion = quaternion_from_euler(roll, pitch, yaw)
pose_target = PoseStamped()
pose_target.pose.position.x = pose_x
pose_target.pose.position.y = pose_y
pose_target.pose.position.z = pose_z
pose_target.pose.orientation.x = quaternion[0]
pose_target.pose.orientation.y = quaternion[1]
pose_target.pose.orientation.z = quaternion[2]
pose_target.pose.orientation.w = quaternion[3]
pose_target.header.frame_id = "base_link"
pose_target.header.stamp = rospy.Time.now()
print "============= PoseStamped message filled out"
# Execute motion
while not rospy.is_shutdown():
result = self.group.moveToPose(pose_target, "gripper_link",
tolerance = 0.02, plan_only=True,
start_state = self.virtual_state)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "================ Pose Achieved"
self.trajectory = result.planned_trajectory.joint_trajectory
return self.trajectory
def plan_to_jointspace_goal(self, pose):
joints = ["shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint",
"forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"]
while not rospy.is_shutdown():
result = self.group.moveToJointPosition(joints, pose, plan_only=True,
start_state=self.virtual_state
)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "============ Pose Achieved"
self.trajectory = result.planned_trajectory #.joint_trajectory
return self.trajectory
def plan_to_gripper_goal(self, posture):
joints = ["gripper_link", "l_gripper_finger_link", "r_gripper_finger_link"]
while not rospy.is_shutdown():
result = self.gripper.moveToJointPosition(joints, pose, plan_only=True,
start_state=self.virtual_state)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Gripper Posture Plan Successful")
self.trajectory = result.planned_trajectory.joint_trajectory
return self.trajectory
def update_virtual_state(self):
# Sets joint names and sets position to final position of previous trajectory
if self.trajectory == None:
print "No trajectory available to provide a new virtual state."
print "Update can only occur after trajectory has been planned."
else:
self.virtual_state.joint_state.name = ["shoulder_pan_joint",
"shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"]
self.virtual_state.joint_state.position = self.trajectory.points[-1].positions
def clear_virtual_state(self):
self.virtual_state.joint_state.joint_names = []
self.virtual_state.joint_state.position = []
def execute_trajectory(self, trajectory):
execute_known_trajectory = rospy.ServiceProxy('execute_known_trajectory', ExecuteKnownTrajectory)
result = execute_known_trajectory(trajectory)
class Robot(object):
"""
fetch max x = 0.96 (wrist roll joint)
"""
def __init__(self):
rospy.loginfo("Waiting for Fetch ...")
self.tuck_the_arm_joints_value = [
0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0
]
self.stow_joints_value = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso")
self.joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
def move_to_pose(self, postion, quaternion, frame='base_link'):
target_frame = 'wrist_roll_link'
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose = Pose(
Point(postion[0], postion[1], postion[2]),
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
if not rospy.is_shutdown():
self.move_group.moveToPose(pose_stamped, target_frame)
result = self.move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Move success!")
return True
else:
rospy.logerr("in state: %s",
self.move_group.get_move_action().get_state())
return False
else:
rospy.logerr("MoveIt! failure no result returned.")
return False
def move_to_joints(self, joints_value):
# Plans the joints in joint_names to angles in pose
self.move_group.moveToJointPosition(self.joint_names,
joints_value,
wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group.get_move_action().wait_for_result()
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("move to joint Success!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
def tuck_the_arm(self):
self.move_to_joints(self.tuck_the_arm_joints_value)
def stow(self):
self.move_to_joints(self.stow_joints_value)
def stop(self):
"""This stops all arm movement goals
It should be called when a program is exiting so movement stops
"""
self.move_group.get_move_action().cancel_all_goals()
class FetchPose:
def __init__(self):
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso")
self.joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
# planning_scene = PlanningSceneInterface("base_link")
# planning_scene.removeCollisionObject("my_front_ground")
# planning_scene.removeCollisionObject("my_back_ground")
# planning_scene.removeCollisionObject("my_right_ground")
# planning_scene.removeCollisionObject("my_left_ground")
# planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
# planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
# planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
# planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.tuck_the_arm_joints_value = [
0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0
]
self.stow_joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
# self.high_pose_value = [0.38380688428878784, -0.12386894226074219, -0.31408262252807617, 2.1759514808654785, 0.0061359405517578125, 0.9556700587272644, -1.921694278717041, -1.6908303499221802]
self.high_pose_value = [
0.4946120488643647, -0.19136428833007812, -0.4793691635131836,
0.009587380103766918, 0.1629854440689087, 0.2983585298061371,
1.8430781364440918, -1.6992675065994263
]
self.trans = Transformer()
def excute(self, joints_value):
if rospy.is_shutdown():
return False
# Plans the joints in joint_names to angles in pose
self.move_group.moveToJointPosition(self.joint_names,
joints_value,
wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group.get_move_action().wait_for_result()
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("pose Success!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
def tuck_the_arm(self):
self.excute(self.tuck_the_arm_joints_value)
def stow(self):
self.excute(self.stow_joints_value)
def hige_pose(self):
self.excute(self.high_pose_value)
def control_torso(self, offset=0.1):
joints_values = self.pose_group.get_current_joint_values()
joints_values[0] += offset
self.excute(joints_values)
def grasp_by_move_group(self, point_xyz):
wrist_roll_link_gripper_offset = 0.20
q1 = self.trans.quaternion_from_direction((1, 0, 0), (0, 0, -1))
q2 = self.trans.quaternion_from_axis(90, (1, 0, 0))
quaternion = quaternion_multiply(q1, q2)
pose = Pose(
Point(point_xyz[0], point_xyz[1],
point_xyz[2] + wrist_roll_link_gripper_offset), # offset
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
gripper_pose_stamped.header.stamp = rospy.Time.now()
gripper_pose_stamped.pose = pose
gripper_frame = 'wrist_roll_link'
self.move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = self.move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print("reach success!")
return 'Success'
else:
rospy.logerr("reach fail")
return 'Fail'
else:
rospy.logerr("MoveIt! failure no result returned.")
return 'Fail'
class FetchArmController:
def __init__(self):
self.moving_mode = False
self.plan_only = False
self.prev_joy_buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
self.joy_subscriber = rospy.Subscriber("/fetch_joy", Joy,
self.joy_callback)
self.joints_subscriber = rospy.Subscriber("/joint_states", JointState,
self.joints_states_callback)
self.rgbimage_subscriber = rospy.Subscriber("/head_camera/depth/image",
Image,
self.depthimage_callback)
self.depthimage_subscriber = rospy.Subscriber(
"/head_camera/depth/image", Image, self.rgbimage_callback)
self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_reset_pos",
Empty,
self.arm_reset_callback)
self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_start_pos",
Empty,
self.arm_start_callback)
self.arm_effort_pub = rospy.Publisher(
"/arm_controller/weightless_torque/command",
JointTrajectory,
queue_size=2)
self.gripper_client = actionlib.SimpleActionClient(
"gripper_controller/gripper_action", GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher(
"/arm_controller/cartesian_twist/command", Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient(
"head_controller/point_head", PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm",
"base_link",
plan_only=self.plan_only)
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def joints_states_callback(self, data):
if len(data.name) == 13:
self.arm_joints = data.position[-7:]
def depthimage_callback(self, image):
self.depth_image = image
def rgbimage_callback(self, image):
self.rgb_image = image
def joy_callback(self, joy):
if not self.moving_mode:
self.arm_control_action(joy)
self.prev_joy_buttons = joy.buttons
def arm_control_action(self, joy):
twist = Twist()
twist.linear.x = 0.2 * joy.axes[1]
twist.linear.y = 0.2 * joy.axes[0]
twist.linear.z = -0.2 * joy.axes[2]
twist.angular.x = 0.2 * joy.axes[5]
twist.angular.y = 0.2 * joy.axes[6]
twist.angular.z = 0.2 * joy.axes[4]
self.arm_cartesian_twist_pub.publish(twist)
if joy.buttons[0] and not self.prev_joy_buttons[0]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.0
self.gripper_client.send_goal(goal)
elif not joy.buttons[0] and self.prev_joy_buttons[0]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
def arm_reset_callback(self, data):
reset_pose_stamped = PoseStamped()
reset_pose_stamped.header.frame_id = "base_link"
reset_pose_stamped.header.stamp = rospy.Time.now()
reset_pose_stamped.pose = Pose(Point(0.055, -0.140, 0.623),
Quaternion(0.460, -0.504, 0.511, 0.523))
self.arm_move_group.moveToPose(reset_pose_stamped, "wrist_roll_link")
def arm_start_callback(self, data):
reset_pose_stamped = PoseStamped()
reset_pose_stamped.header.frame_id = "base_link"
reset_pose_stamped.header.stamp = rospy.Time.now()
reset_pose_stamped.pose = Pose(Point(0.213, -0.520, 0.605),
Quaternion(0.545, 0.766, -0.197, 0.278))
self.arm_move_group.moveToPose(reset_pose_stamped, "wrist_roll_link")
class TrajectorySimulator():
def __init__(self):
# Instantiate a RobotCommander object.
# This object is an interface to the robot as a whole.
#robot = moveit_commander.RobotCommander()
# Instantiate a MoveGroupCommander object.
# This object is an interface to one 'group' of joints,
# in our case the joints of the arm.
self.group = MoveGroupInterface("arm", "base_link")
# Create DisplayTrajectory publisher to publish trajectories
# for RVIZ to visualize.
# self.display_trajectory_publisher = rospy.Publisher(
#"/move_group/display_planned_path",
#moveit_msgs.msg.DisplayTrajectory)
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(15)
print "============ Starting tutorial "
def get_state_info(self):
# Let's get some basic information about the robot.
# This info may be helpful for debugging.
# Get name of reference frame for this robot
print "============ Reference frame: %s" % self.group.get_planning_frame(
)
# Get name of end-effector link for this group
print "============ Reference frame: %s" % self.group.get_end_effector_link(
)
# List all groups defined on the robot
print "============ Robot Groups:"
print self.robot.get_group_names()
# Print entire state of the robot
print "============ Printing robot state"
print self.robot.get_current_state()
print "============"
def plan_to_pose_goal(self, goal):
### Planning to a Pose Goal
# Let's plan a motion for this group to a desired pose for the
# end-effector
print "============ Generating plan 1"
# Extract goal position
pose_x = goal[0]
pose_y = goal[1]
pose_z = goal[2]
# Extract goal orientation--provided in Euler angles
roll = goal[3]
pitch = goal[4]
yaw = goal[5]
# Convert Euler angles to quaternion, create PoseStamped message
quaternion = quaternion_from_euler(roll, pitch, yaw)
pose_target = PoseStamped()
pose_target.pose.position.x = pose_x
pose_target.pose.position.y = pose_y
pose_target.pose.position.z = pose_z
pose_target.pose.orientation.x = quaternion[0]
pose_target.pose.orientation.y = quaternion[1]
pose_target.pose.orientation.z = quaternion[2]
pose_target.pose.orientation.w = quaternion[3]
pose_target.header.frame_id = "base_link"
pose_target.header.stamp = rospy.Time.now()
print "============= PoseStamped message filled out"
# Execute motion
while not rospy.is_shutdown():
result = self.group.moveToPose(pose_target,
"gripper_link",
tolerance=0.3,
plan_only=False)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "================ Pose Achieved"
return
# Now we call the planner to compute the plan and visualize it if successful.
# We are just planning here, not asking move_group to actually move the robot.
print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(5)
print "============ Goal achieved"
# group.plan() automatically asks RVIZ to visualize the plan,
# so we need not explicitly ask RVIZ to do this.
# For completeness of understanding, however, we make this
# explicit request below. The same trajectory should be displayed
# a second time.
print "============ Visualizing plan1"
"""
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)
"""
def plan_to_jointspace_goal(self, pose):
# Let's set a joint-space goal and visualize it in RVIZ
# A joint-space goal differs from a pose goal in that
# a goal for the state of each joint is specified, rather
# than just a goal for the end-effector pose, with no
# goal specified for the rest of the arm.
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
while not rospy.is_shutdown():
result = self.group.moveToJointPosition(joints, pose, 0.1)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "============ Pose Achieved"
rospy.sleep(5)
return "Success"
class GraspingClient(object):
def __init__(self):
self.arm = MoveGroupInterface('arm_with_torso', 'map')
self.move_group = MoveGroupInterface('arm', 'base_link')
def pick(self, target_pose):
#print self.arm._fixed_frame, self.arm.planner_id
#Taking coordinate values of the object
object_x, object_y, object_z = round(
target_pose.pose.position.x,
2), round(target_pose.pose.position.y,
2), round(target_pose.pose.position.z, 2)
print object_x, object_y, object_z
#Initializing offset values for translation
offset_x, offset_y, offset_z = 0, 0, 0.05
#Initializing offset values for rotation
roll, pitch, yaw = 0, 0, 0
#Applying rotation to the arm
for i in range(10, 1, -1):
#Adding translation along x-axis
final_x = object_x + offset_x
#Adding translation along y-axis
final_y = object_y + offset_y
#Adding translation along z-axis
final_z = object_z + offset_z + i * 0.1
print final_x, final_y, final_z, i * 0.1
final_quaternion = quaternion_from_euler(radians(roll),
radians(pitch),
radians(yaw), 'sxyz')
pose_goal = PoseStamped()
pose_goal.header.frame_id = 'map'
pose_goal.pose.position.x = final_x
pose_goal.pose.position.y = final_y
pose_goal.pose.position.z = final_z
pose_goal.pose.orientation.x = final_quaternion[0]
pose_goal.pose.orientation.y = final_quaternion[1]
pose_goal.pose.orientation.z = final_quaternion[2]
pose_goal.pose.orientation.w = final_quaternion[3]
pose_goal.header.stamp = rospy.Time.now()
self.arm.moveToPose(pose_goal, 'wrist_roll_link')
result = self.arm.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo('Moved arm')
rospy.sleep(10)
else:
rospy.logerr('Cannot move arm')
else:
rospy.logerr('No result')
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo('Tucking done')
class moveit_example:
def __init__(self):
# initialize ROS node
self.picking = False
rospy.init_node('eexxaammple', anonymous=True)
# create variables for the arms
self.left_arm = baxter_interface.Limb('left')
self.right_arm = baxter_interface.Limb('right')
# print self.left_arm.joint_angles()
# i = 0;
# while (i == 0):
# print(self.left_arm.joint_efforts())
# rospy.sleep(.5)
#release_object(self)
rospy.Subscriber('/object_poses', PoseArray,
self.object_poses_callback)
self.calibrated = False
# initialize arm group variables for MoveIt
self.both_arms_group = MoveGroupInterface("both_arms", "base")
self.right_arm_group = MoveGroupInterface("right_arm", "base")
self.left_arm_group = MoveGroupInterface("left_arm", "base")
# create variables for the grippers
self.leftgripper = baxter_interface.Gripper('left')
self.rightgripper = baxter_interface.Gripper('right')
# calibrate left gripper and keep it open
self.leftgripper.calibrate()
self.leftgripper.open()
# calibrate right gripper and keep it open
self.rightgripper.calibrate()
self.rightgripper.open()
# names of the left and right joints
self.both_arm_joints = [
'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0',
'left_s1', 'right_s0', 'right_s1', 'right_w0', 'right_w1',
'right_w2', 'right_e0', 'right_e1'
]
self.right_joints = [
'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2',
'right_e0', 'right_e1'
]
self.left_joints = [
'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0',
'left_s1'
]
self.object_poses = None
# joint angles for initial position of arms
# these were manually recorded. I essentially just moved the arms to the positions
# I desired and looked up their joint angles.
self.init_joint_angles = [
0.2515728492132078, 1.58958759144626, 3.0418839023767754,
-1.2862428906419194, 1.2916118233995184, 0.37735927381981177,
-1.3203739631723699, -0.1514806028036846, -1.2908448330055757,
2.908044078633773, -1.5424176822187836, -0.34131072530450457,
0.984048675428493, 1.1949710337627373
]
self.tug = [
0.7294078646395142, 1.1850001586414822, 1.957359485341788,
-0.9821311994436361, 1.5397332158399841, -0.7742768026851626,
-0.7566360236244803
]
# joint angles for pre-grasping poses of the left and right arms.
# These were manually retrieved. I essentially just moved the arms to the desired
# positions and read off their joint angles
self.preright = [
0.27381557063754636, -0.7309418454273996, 2.269908070873441,
-1.0181797479589434, 0.9177040063524488, 1.1213399559442374,
1.878359474765689
]
self.preleft = [
0.7294078646395142, 1.1850001586414822, 1.957359485341788,
-0.9821311994436361, 1.5397332158399841, -0.7742768026851626,
-0.7566360236244803
]
self.pregrasp_pose = [
0.3996019952441503, 1.7648448964621686, 2.2507333110248733,
-1.0599807244288209, 1.1293933550806359, -0.6764855274574675,
-1.113286556807839
]
# Here, I manually specify the 3D position and orientation of the object.
# I hard-code the position here.
# This is possibly the only functionality yout'd modify once the vision system is running.
# Once the vision system is running, it will publish the poses of candidate objects to
# a ROS topic. Your job would be to to read pose messages from the topic, select a pose
# from the list and feed it to moveit.
#
self.pickgoal = PoseStamped()
self.pickgoal.header.frame_id = "base"
self.pickgoal.header.stamp = rospy.Time.now()
self.pickgoal.pose.position.x = 0.57
self.pickgoal.pose.position.y = -0.1
self.pickgoal.pose.position.z = -0.05
self.pickgoal.pose.orientation.x = 1.0
self.pickgoal.pose.orientation.y = 0.0
self.pickgoal.pose.orientation.z = 0.0
self.pickgoal.pose.orientation.w = 0.0
######### THIS IS WHERE THE ARM MOTION HAPPENS ##############
# Move both arms to their initial positions. Then wait for 5 seconds (rospy.sleep(5))
self.both_arms_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
# rospy.sleep(5)
# # Move the left arm to the pre-grasping position so that the grippers hover over
# # the object
# self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False)
# # Move arm to the object
# self.left_arm_group.moveToPose(self.pickgoal, "left_gripper", plan_only=False)
# # close the left gripper
# self.leftgripper.close()
# self.preleft = [0.41302432713814763, 1.419315723990979, 2.2227381616459647, -0.724805922275858,
# 1.2198982215658754, -0.947233136519243, -0.9100341024130217]
# Move arm back to the pre-grasping position. At this point the object should be
# between the grippers
# self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False)
# self.preleft = [0.336325287743877, 1.1060001480653834, 2.5651993725413833, -0.7156020375485456,
# 1.7215099392044055, -0.45099035163831164, -1.2241166687325602]
# self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False)
# self.leftgripper.open()
rospy.sleep(3)
for i in range(5):
self.pick_arbitrary_object()
rospy.sleep(2)
rospy.spin()
def object_poses_callback(self, data):
if not self.picking:
N = len(data.poses)
if N > 0:
self.object_poses = data.poses
else:
self.object_poses = None
def pick_arbitrary_object(self):
if self.object_poses is not None:
N = len(self.object_poses)
target = random.sample(self.object_poses, 1)[0]
self.pick_up_and_put_back(target)
def pick_up_and_put_back(self, pose):
stamped = PoseStamped()
stamped.header.frame_id = "base"
stamped.header.stamp = rospy.Time.now()
stamped.pose = pose
self.picking = True
#pick up
self.left_arm_group.moveToJointPosition(self.left_joints,
self.pregrasp_pose,
plan_only=False)
self.left_arm_group.moveToPose(stamped,
"left_gripper",
plan_only=False)
self.leftgripper.close()
self.left_arm_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
rospy.sleep(2)
#put back
self.left_arm_group.moveToJointPosition(self.left_joints,
self.pregrasp_pose,
plan_only=False)
self.left_arm_group.moveToPose(stamped,
"left_gripper",
plan_only=False)
self.leftgripper.open()
self.left_arm_group.moveToJointPosition(self.left_joints,
self.pregrasp_pose,
plan_only=False)
self.left_arm_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
self.picking = False
rospy.sleep(2)
def main():
rospy.init_node('move_group_python_path_follow', anonymous=True)
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
moveit_commander.roscpp_initialize(sys.argv)
move_group = MoveGroupInterface("arm", "base_link")
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm")
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Printing robot state"
print robot.get_current_state()
print "============"
gripper_frame = 'wrist_roll_link'
gripper_poses = list()
radius = 0.2
for theta in np.arange(-pi, pi, 0.5 * pi):
gripper_poses.append(
Pose(
Point(0.75, radius * np.sin(theta),
0.6 + radius * np.cos(theta)),
Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))))
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
gripper_pose_stamped.header.stamp = rospy.Time.now()
gripper_pose_stamped.pose = gripper_poses[0]
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Success!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
trajectory = RobotTrajectory()
(plan, fraction) = group.compute_cartesian_path(gripper_poses, 0.01, 0.0)
group.execute(plan)
move_group.get_move_action().cancel_all_goals()
def move_corner(self, x, y):
position = self.cal_position.get_base_position_from_pix(x, y)
position[0] = position[0] - 0.20
move_group = MoveGroupInterface("arm_with_torso", "base_link")
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# This is the wrist link not the gripper itself
gripper_frame = 'wrist_roll_link'
pose = Pose(Point(position[0], position[1], position[2]),
Quaternion(0, 0, 0, 1))
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
# Finish building the Pose_stamped message
# If the message stamp is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
gripper_pose_stamped.pose = pose
# Move gripper frame to the pose specified
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Hello there!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
time.sleep(1)
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
move_group.moveToJointPosition(joint_names, joints_value, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("pose Success!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
class TrajectoryGenerator():
# Wrapper class that facilitates trajectory planning.
# Includes methods for planning to arbitrary joint-space goals
# and end-effector pose goals.
def __init__(self):
self.group = MoveGroupInterface("arm", "base_link", plan_only=True)
self.gripper = MoveGroupInterface("gripper",
"base_link",
plan_only=True)
self.virtual_arm_state = RobotState()
self.virtual_gripper_state = RobotState()
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(5)
print "============ Starting planning"
def generate_trajectories(self, motion_goals, **kwargs):
approved_trajectories = list()
supported_args = ("get_approval")
for arg in kwargs.keys():
if arg not in supported_args:
rospy.loginfo(
"generate_trajectories: unsupported argument: %s", arg)
if type(motion_goals) is not type(list()):
motion_goals = [motion_goals]
for motion_goal in motion_goals:
if motion_goal.goal_type is 'ee_pose':
print "==== Virtual Start State: ", self.virtual_arm_state
trajectory = self.plan_to_ee_pose_goal(motion_goal.pose)
elif motion_goal.goal_type is "jointspace":
trajectory = self.plan_to_jointspace_goal(motion_goal.state)
elif motion_goal.goal_type is "gripper_posture":
trajectory = self.plan_to_gripper_goal(motion_goal.posture)
else:
rospy.loginfo(
"Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'"
)
sys.exit()
try:
kwargs["get_approval"]
approved = self.get_user_approval()
if approved:
approved_trajectories.append(trajectory)
self.update_virtual_state(trajectory)
else:
# Recur
approved_trajectories += self.generate_trajectories(
motion_goal, get_approval=True)
except KeyError:
pass
returnable_approved_trajectories = copy.deepcopy(approved_trajectories)
return returnable_approved_trajectories
def get_user_approval(self):
choice = 'not_set'
options = {'y': True, 'r': False}
while choice.lower() not in options.keys():
choice = raw_input("Add plan to execution queue? Answer 'y' for yes "\
" or 'r' for replan: ")
return options[choice]
def plan_to_ee_pose_goal(self, goal):
# Extract goal position
pose_x = goal[0]
pose_y = goal[1]
pose_z = goal[2]
# Extract goal orientation--provided in Euler angles
roll = goal[3]
pitch = goal[4]
yaw = goal[5]
# Convert Euler angles to quaternion, create PoseStamped message
quaternion = quaternion_from_euler(roll, pitch, yaw)
pose_target = PoseStamped()
pose_target.pose.position.x = pose_x
pose_target.pose.position.y = pose_y
pose_target.pose.position.z = pose_z
pose_target.pose.orientation.x = quaternion[0]
pose_target.pose.orientation.y = quaternion[1]
pose_target.pose.orientation.z = quaternion[2]
pose_target.pose.orientation.w = quaternion[3]
pose_target.header.frame_id = "base_link"
pose_target.header.stamp = rospy.Time.now()
print "============= PoseStamped message filled out"
# Execute motion
while not rospy.is_shutdown():
result = self.group.moveToPose(pose_target,
"wrist_roll_link",
tolerance=0.02,
plan_only=True,
start_state=self.virtual_arm_state,
planner_id="PRMkConfigDefault")
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "================ Pose Achieved"
self.trajectory = result.planned_trajectory.joint_trajectory
return result.planned_trajectory
def plan_to_jointspace_goal(self, state):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
while not rospy.is_shutdown():
result = self.group.moveToJointPosition(
joints,
state,
plan_only=True,
start_state=self.virtual_arm_state,
planner_id="PRMkConfigDefault")
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "============ Pose Achieved"
self.trajectory = result.planned_trajectory.joint_trajectory
return result.planned_trajectory
def plan_to_gripper_goal(self, posture):
joints = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
while not rospy.is_shutdown():
result = self.gripper.moveToJointPosition(
joints,
posture,
plan_only=True,
start_state=self.virtual_gripper_state)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Gripper Posture Plan Successful")
self.trajectory = result.planned_trajectory.joint_trajectory
return result.planned_trajectory
def update_virtual_state(self, trajectory):
# Sets joint names and sets position to final position of previous trajectory
if len(trajectory.joint_trajectory.points[-1].positions) == 7:
self.virtual_arm_state.joint_state.name = [
"shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint",
"forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"
]
self.virtual_arm_state.joint_state.position = trajectory.joint_trajectory.points[
-1].positions
rospy.loginfo("Virtual Arm State Updated")
print self.virtual_arm_state.joint_state.position
elif len(trajectory.joint_trajectory.points[-1].positions) == 2:
self.virtual_gripper_state.joint_state.name = [
"l_gripper_finger_joint", "r_gripper_finger_joint"
]
self.virtual_gripper_state.joint_state.position = trajectory.joint_trajectory.points[
-1].positions
rospy.loginfo("Virtual Gripper State Updated")
print self.virtual_gripper_state.joint_state.position
def clear_virtual_arm_state(self):
self.virtual_arm_state.joint_state.name = []
self.virtual_arm_state.joint_state.position = []
def clear_virtual_gripper_state(self):
self.virtual_gripper_state.joint_state.names = []
self.virtual_gripper_state.joint_state.position = []
def get_virtual_arm_state(self):
virtual_arm_state = copy.deepcopy(self.virtual_arm_state)
return virtual_arm_state
def get_virtual_gripper_state(self):
virtual_gripper_state = copy.deepcopy(self.virtual_gripper_state)
return virtual_gripper_state
# gripper_pose_grasp = PoseStamped()
# gripper_pose_grasp.header.frame_id = "base_link"
# gripper_pose_grasp.pose.position = object_pose.g_pos.position
# gripper_pose_grasp.pose.orientation = object_pose.g_pos.orientation
# gripper_pose_grasp.pose.position.x = g_pos_x - Lwrist2gripper
# print(object_pose)
print('created msgs')
print(gripper_pose_pregrasp)
print(gripper_pose_grasp)
print(gripper_pose_up)
# Pregrasp
gripper_pose_pregrasp.header.stamp = rospy.Time.now()
move_group.moveToPose(gripper_pose_pregrasp, gripper_frame)
result = move_group.get_move_action().get_result()
print('moving to pregrasp')
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Moved to Pregrasp")
print()
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
class TrajectoryGenerator():
# Wrapper class that facilitates trajectory planning.
# Includes methods for planning to arbitrary joint-space goals
# and end-effector pose goals.
def __init__(self):
self.group = MoveGroupInterface("arm", "base_link", plan_only=True)
self.gripper = MoveGroupInterface("gripper", "base_link", plan_only=True)
self.virtual_arm_state = RobotState()
self.virtual_gripper_state = RobotState()
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(5)
print "============ Starting planning"
def generate_trajectories(self, motion_goals, **kwargs):
approved_trajectories = list()
supported_args = ("get_approval")
for arg in kwargs.keys():
if arg not in supported_args:
rospy.loginfo("generate_trajectories: unsupported argument: %s",
arg)
if type(motion_goals) is not type(list()):
motion_goals = [motion_goals]
for motion_goal in motion_goals:
if motion_goal.goal_type is 'ee_pose':
print "==== Virtual Start State: ", self.virtual_arm_state
trajectory = self.plan_to_ee_pose_goal(motion_goal.pose)
elif motion_goal.goal_type is "jointspace":
trajectory = self.plan_to_jointspace_goal(motion_goal.state)
elif motion_goal.goal_type is "gripper_posture":
trajectory = self.plan_to_gripper_goal(motion_goal.posture)
else:
rospy.loginfo("Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'")
sys.exit()
try:
kwargs["get_approval"]
approved = self.get_user_approval()
if approved:
approved_trajectories.append(trajectory)
self.update_virtual_state(trajectory)
else:
# Recur
approved_trajectories += self.generate_trajectories(motion_goal, get_approval=True)
except KeyError:
pass
returnable_approved_trajectories = copy.deepcopy(approved_trajectories)
return returnable_approved_trajectories
def get_user_approval(self):
choice = 'not_set'
options = {'y':True, 'r':False}
while choice.lower() not in options.keys():
choice = raw_input("Add plan to execution queue? Answer 'y' for yes "\
" or 'r' for replan: ")
return options[choice]
def plan_to_ee_pose_goal(self, goal):
# Extract goal position
pose_x = goal[0]
pose_y = goal[1]
pose_z = goal[2]
# Extract goal orientation--provided in Euler angles
roll = goal[3]
pitch = goal[4]
yaw = goal[5]
# Convert Euler angles to quaternion, create PoseStamped message
quaternion = quaternion_from_euler(roll, pitch, yaw)
pose_target = PoseStamped()
pose_target.pose.position.x = pose_x
pose_target.pose.position.y = pose_y
pose_target.pose.position.z = pose_z
pose_target.pose.orientation.x = quaternion[0]
pose_target.pose.orientation.y = quaternion[1]
pose_target.pose.orientation.z = quaternion[2]
pose_target.pose.orientation.w = quaternion[3]
pose_target.header.frame_id = "base_link"
pose_target.header.stamp = rospy.Time.now()
print "============= PoseStamped message filled out"
# Execute motion
while not rospy.is_shutdown():
result = self.group.moveToPose(pose_target, "wrist_roll_link",
tolerance = 0.02, plan_only=True,
start_state = self.virtual_arm_state,
planner_id = "PRMkConfigDefault")
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "================ Pose Achieved"
self.trajectory = result.planned_trajectory.joint_trajectory
return result.planned_trajectory
def plan_to_jointspace_goal(self, state):
joints = ["shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint",
"forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"]
while not rospy.is_shutdown():
result = self.group.moveToJointPosition(joints, state, plan_only=True,
start_state=self.virtual_arm_state,
planner_id = "PRMkConfigDefault")
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "============ Pose Achieved"
self.trajectory = result.planned_trajectory.joint_trajectory
return result.planned_trajectory
def plan_to_gripper_goal(self, posture):
joints = ["l_gripper_finger_joint",
"r_gripper_finger_joint"]
while not rospy.is_shutdown():
result = self.gripper.moveToJointPosition(joints, posture, plan_only=True,
start_state=self.virtual_gripper_state)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Gripper Posture Plan Successful")
self.trajectory = result.planned_trajectory.joint_trajectory
return result.planned_trajectory
def update_virtual_state(self, trajectory):
# Sets joint names and sets position to final position of previous trajectory
if len(trajectory.joint_trajectory.points[-1].positions) == 7:
self.virtual_arm_state.joint_state.name = [
"shoulder_pan_joint",
"shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"]
self.virtual_arm_state.joint_state.position = trajectory.joint_trajectory.points[-1].positions
rospy.loginfo("Virtual Arm State Updated")
print self.virtual_arm_state.joint_state.position
elif len(trajectory.joint_trajectory.points[-1].positions) == 2:
self.virtual_gripper_state.joint_state.name = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.virtual_gripper_state.joint_state.position = trajectory.joint_trajectory.points[-1].positions
rospy.loginfo("Virtual Gripper State Updated")
print self.virtual_gripper_state.joint_state.position
def clear_virtual_arm_state(self):
self.virtual_arm_state.joint_state.name = []
self.virtual_arm_state.joint_state.position = []
def clear_virtual_gripper_state(self):
self.virtual_gripper_state.joint_state.names = []
self.virtual_gripper_state.joint_state.position = []
def get_virtual_arm_state(self):
virtual_arm_state = copy.deepcopy(self.virtual_arm_state)
return virtual_arm_state
def get_virtual_gripper_state(self):
virtual_gripper_state = copy.deepcopy(self.virtual_gripper_state)
return virtual_gripper_state
class GraspingClient(object):
def __init__(self, sim=False):
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.robot = moveit_commander.RobotCommander()
self._listener = tf.TransformListener()
# self.frame = self._listener.lookupTransform('/left_ee_link', '/base_link', rospy.Time(0))
# rospy.loginfo("========== Left_ee to base_link is: ")
# rospy.loginfo(self.frame)
# curr_pos = self._listener.lookupTransform('/base_link', '/left_ee_link', rospy.Time(0))
# rospy.loginfo(curr_pos)
# self.move_group = moveit_commander.MoveGroupCommander("upper_body")
# rospy.loginfo("======upper_body_group connected =========")
# rospy.loginfo(self.move_group.get_joints())
# self.lmove_group = moveit_commander.MoveGroupCommander("left_arm")
# rospy.loginfo("======left_arm_group connected =========")
# self.rmove_group = moveit_commander.MoveGroupCommander("right_arm")
# rospy.loginfo("======right_arm_group connected =========")
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
if "6dof" == self.dof:
rospy.logwarn(
"======= Please change launch param to 7DoF =========")
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
# self.constrained_stow =[-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0]
# flipping pose
self.constrained_stow = [
-0.23, -0.71, -1.02, -1.0, 0.9, 1.89, -2.41, 0.44, 0.71, 1.12,
1.21, -1.02, -1.84, 2.61, 0.20, 0, 0
]
# self.constrained_stow_2 = [-0.34, -0.66, -0.92, -1.18, 0.94, 1.75, -2.49, 0.54, 0.65, 1.0, 1.35, -1.07, -1.75, 2.67, 0.20, 0, 0]
self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
self.tableDist = 0.8
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
'''
pickplace = [lg_itf, rg_itf]
pick_result = [None, None]
'''
self.pickplace = [None] * 2
self.pickplace[0] = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace[0].planner_id = "RRTConnectkConfigDefault"
self.pickplace[1] = PickPlaceInterface("right_side",
"right_gripper",
verbose=True)
self.pickplace[1].planner_id = "RRTConnectkConfigDefault"
self.pick_result = [None] * 2
self._last_gripper_picked = 0
self.place_result = [None] * 2
self._last_gripper_placed = 0
self._objs_to_keep = []
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
rospy.loginfo(
"============= FindGraspableObjectsAction connected! ============")
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
def add_objects_to_keep(self, obj):
self._objs_to_keep.append(obj)
def clearScene(self):
self.scene.clear()
def getPickCoordinates(self):
self.updateScene(0, False)
nut, grasps = self.getGraspableNut(False)
bolt, grasps = self.getGraspableBolt(False)
if (None == nut) or (None == bolt):
rospy.loginfo("======= No nut or bolt found ========")
return None
center_objects = (nut.primitive_poses[0].position.y +
bolt.primitive_poses[0].position.y) / 2
surface = self.getSupportSurface(nut.support_surface)
tmp1 = surface.primitive_poses[
0].position.x - surface.primitives[0].dimensions[0] / 2
surface = self.getSupportSurface(bolt.support_surface)
tmp2 = surface.primitive_poses[
0].position.x - surface.primitives[0].dimensions[0] / 2
front_edge = (tmp1 + tmp2) / 2
coords = Pose2D(x=(front_edge - self.tableDist),
y=center_objects,
theta=0.0)
return coords
def getGBCoordinates(self):
'''
Get gearbox pos from arv marker
'''
pass
def updateScene(self, gripper=0, plan=True):
# find objects
rospy.loginfo("Updating scene...")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = plan
goal.gripper = gripper
# send gripper to FindGraspableObject
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
# return graspable object: objects, grasps(moveit)
find_result = self.find_client.get_result()
rospy.loginfo("=========== before updating, find_result.objects is: ")
rospy.loginfo(find_result.objects)
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, True)
# insert objects to scene
idx = -1
# result.object = GraspableObject = object, grasps
# obj.object = grasp_msgs.object
for obj in find_result.objects:
if obj.object.primitive_poses[
0].position.z < 0.5 or obj.object.primitive_poses[
0].position.x > 2.0 or obj.object.primitive_poses[
0].position.y > 0.5:
continue
idx += 1
obj.object.name = "object%d_%d" % (idx, gripper)
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=True)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
if obj.primitive_poses[0].position.z < 0.5:
continue
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
obj.primitives[0].dimensions[1], # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=True)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableNut(self, planned=True):
if self.objects == None:
rospy.loginfo("============= No self.objects ===========")
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1 and planned:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5 or \
obj.object.primitive_poses[0].position.z > 1.0 or \
obj.object.primitive_poses[0].position.x > 2.0:
continue
elif (obj.object.primitives[0].type == sp.CYLINDER):
if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.102 or \
obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.142:
continue
elif (obj.object.primitives[0].type == sp.BOX):
if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.102 or \
obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.142:
continue
else:
continue
print "nut: ", obj.object.primitive_poses[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getGraspableBolt(self, planned=True):
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1 and planned:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5 or \
obj.object.primitive_poses[0].position.z > 1.0 or \
obj.object.primitive_poses[0].position.x > 2.0:
continue
elif (obj.object.primitives[0].type == sp.CYLINDER):
if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.21 or \
obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.28:
continue
elif (obj.object.primitives[0].type == sp.BOX):
if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.21 or \
obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.28:
continue
else:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps, gripper=0):
success, pick_result = self.pickplace[gripper].pick_with_retry(
block.name,
grasps,
retries=1,
support_name=block.support_surface,
scene=self.scene)
self.pick_result[gripper] = pick_result
self._last_gripper_picked = gripper
return success
def place(self, block, pose_stamped, gripper=0):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result[
gripper].grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result[
gripper].grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result[
gripper].grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace[gripper].place_with_retry(
block.name, places, retries=1, scene=self.scene)
self.place_result[gripper] = place_result
self._last_gripper_placed = gripper
return success
def goto_tuck(self):
# remove previous objects
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints,
self.tucked,
0.05,
max_velocity_scaling_factor=0.3)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
while not rospy.is_shutdown():
try:
result = self.move_group.moveToJointPosition(
self._upper_body_joints,
self.constrained_stow,
0.01,
max_velocity_scaling_factor=0.3)
except:
continue
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp_lx(self):
plan_lx = PoseStamped()
plan_lx.header.frame_id = 'base_link'
plan_lx.pose.position.x = 0.71
plan_lx.pose.position.y = 0.435
plan_lx.pose.position.z = 1.34
while not rospy.is_shutdown():
try:
result = self.lmove_group.moveToPose(plan_lx, 'left_ee_link',
0.05)
except:
continue
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def left_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "left_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.lmove_group.moveToJointPosition(
self._left_arm_joints,
self.larm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def right_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "right_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.rmove_group.moveToJointPosition(
self._right_arm_joints,
self.rarm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def open_gripper(self):
self._lgripper.command(self._gripper_open, block=True)
self._rgripper.command(self._gripper_open, block=True)
def close_gripper(self):
self._lgripper.command(self._gripper_closed, block=True)
self._rgripper.command(self._gripper_closed, block=True)
class GripperController(object):
def __init__(self):
self.client = actionlib.SimpleActionClient(
"gripper_controller/gripper_action", GripperCommandAction)
rospy.loginfo("Waiting for gripper_action...")
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
self.goal = GripperCommandGoal()
self.client.wait_for_server()
def close(self):
'''Close the gripper'''
self.goal.command.position = 0
self.goal.command.max_effort = 100
self.client.send_goal(self.goal)
self.client.wait_for_result()
def release(self):
'''Release the gripper'''
self.goal.command.position = 1
self.goal.command.max_effort = 100
self.client.send_goal(self.goal)
self.client.wait_for_result()
def tuck(self):
'''Tuck the arm of fetch'''
position = (0.0555, -0.138, 0.571)
quaternion = (0.45848, -0.50438, 0.5078, 0.5268)
self.move_to_pose(position, quaternion)
def move_to_pose(self, postion, quaternion, frame='base_link'):
target_frame = 'wrist_roll_link'
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose = Pose(
Point(postion[0], postion[1], postion[2]),
Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
if not rospy.is_shutdown():
self.move_group.moveToPose(pose_stamped, target_frame)
result = self.move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Move success!")
else:
rospy.logerr("in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# self.move_group.get_move_action().cancel_all_goals()
def move_to_some_pose(self, postion, direction, frame='base_link'):
target_frame = 'wrist_roll_link'
quatrenion = Transformer().quaternion_from_direction((1, 0, 0),
direction)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose = Pose(Point(postion[0], postion[1], postion[2]),
quatrenion)
if not rospy.is_shutdown():
self.move_group.moveToPose(pose_stamped, target_frame)
result = self.move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("See!")
else:
rospy.logerr("in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
self.move_group.get_move_action().cancel_all_goals()
def wave(self):
'''Just for fun'''
gripper_poses = [
((0.347, 0.045, 1.022), (-0.174, -0.301, 0.273, 0.635)),
((0.347, 0.245, 1.022), (-0.274, -0.701, 0.173, 0.635))
]
for i in range(5):
for pose in gripper_poses:
self.move_to_pose(pose[0], pose[1])
self.stop()
def stop(self):
"""This stops all arm movement goals
It should be called when a program is exiting so movement stops
"""
self.move_group.get_move_action().cancel_all_goals()
class Sensory_data_collection:
def __init__(self):
rospy.init_node('pnp', anonymous=True)
self.both_arms_group = MoveGroupInterface("both_arms", "base")
self.right_arm_group = MoveGroupInterface("right_arm", "base")
self.left_arm_group = MoveGroupInterface("left_arm", "base")
self.leftgripper = baxter_interface.Gripper('left')
self.rightgripper = baxter_interface.Gripper('right')
self.leftgripper.calibrate()
self.leftgripper.open()
self.rightgripper.calibrate()
self.rightgripper.open()
self.both_arm_joints = [
'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0',
'left_s1', 'right_s0', 'right_s1', 'right_w0', 'right_w1',
'right_w2', 'right_e0', 'right_e1'
]
self.right_joints = [
'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2',
'right_e0', 'right_e1'
]
self.left_joints = [
'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0',
'left_s1'
]
self.init_joint_angles = [
0.2515728492132078, 1.58958759144626, 3.0418839023767754,
-1.2862428906419194, 1.2916118233995184, 0.37735927381981177,
-1.3203739631723699, -0.1514806028036846, -1.2908448330055757,
2.908044078633773, -1.5424176822187836, -0.34131072530450457,
0.984048675428493, 1.1949710337627373
]
self.preright = [
0.27381557063754636, -0.7309418454273996, 2.269908070873441,
-1.0181797479589434, 0.9177040063524488, 1.1213399559442374,
1.878359474765689
]
self.preleft = [
0.7294078646395142, 1.1850001586414822, 1.957359485341788,
-0.9821311994436361, 1.5397332158399841, -0.7742768026851626,
-0.7566360236244803
]
self.pickgoal = PoseStamped()
self.pickgoal.header.frame_id = "base"
self.pickgoal.header.stamp = rospy.Time.now()
self.pickgoal.pose.position.x = 0.57
self.pickgoal.pose.position.y = -0.1
self.pickgoal.pose.position.z = -0.05
self.pickgoal.pose.orientation.x = 1.0
self.pickgoal.pose.orientation.y = 0.0
self.pickgoal.pose.orientation.z = 0.0
self.pickgoal.pose.orientation.w = 0.0
rospy.Subscriber('/joy', Joy, self.joystick_callback)
def joystick_callback(self, data):
if data.buttons[3] == 1:
arm = 'left'
self.pick_and_place(arm)
elif data.buttons[1] == 1:
arm = 'left'
#shake
elif data.buttons[4] == 1:
arm = 'left'
self.pick_and_drop(arm)
elif data.buttons[0] == 1:
arm = 'left'
#push
def pick_and_drop(self, arm):
# leftLimb = baxter_interface.Limb('left')
# rightLimb = baxter_interface.Limb('right')
# print leftLimb.joint_angles()
# print " "
# print rightLimb.joint_angles()
# '''
# Move both arms to start state
self.both_arms_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
#pick and drop
if arm == 'left':
self.left_arm_group.moveToJointPosition(self.left_joints,
self.preleft,
plan_only=False)
rospy.sleep(1)
self.left_arm_group.moveToPose(self.pickgoal,
"left_gripper",
plan_only=False)
self.leftgripper.close()
rospy.sleep(3)
self.left_arm_group.moveToJointPosition(self.left_joints,
self.preleft,
plan_only=False)
rospy.sleep(1)
self.leftgripper.open()
else:
self.right_arm_group.moveToJointPosition(self.right_joints,
self.preright,
plan_only=False)
rospy.sleep(1)
self.right_arm_group.moveToPose(self.pickgoal,
"right_gripper",
plan_only=False)
self.rightgripper.close()
rospy.sleep(3)
self.right_arm_group.moveToJointPosition(self.right_joints,
self.preright,
plan_only=False)
rospy.sleep(1)
self.rightgripper.open()
rospy.sleep(1)
self.both_arms_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
def pick_and_place(self, arm):
# leftLimb = baxter_interface.Limb('left')
# rightLimb = baxter_interface.Limb('right')
# print leftLimb.joint_angles()
# print " "
# print rightLimb.joint_angles()
# '''
# Move both arms to start state
self.both_arms_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
#pick
if arm == 'left':
self.left_arm_group.moveToJointPosition(self.left_joints,
self.preleft,
plan_only=False)
rospy.sleep(1)
self.left_arm_group.moveToPose(self.pickgoal,
"left_gripper",
plan_only=False)
self.leftgripper.close()
else:
self.right_arm_group.moveToJointPosition(self.right_joints,
self.preright,
plan_only=False)
rospy.sleep(1)
self.right_arm_group.moveToPose(self.pickgoal,
"right_gripper",
plan_only=False)
self.rightgripper.close()
rospy.sleep(1)
self.both_arms_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
rospy.sleep(1)
#place
if arm == 'left':
self.left_arm_group.moveToJointPosition(self.left_joints,
self.preleft,
plan_only=False)
self.left_arm_group.moveToPose(self.pickgoal,
"left_gripper",
plan_only=False)
self.leftgripper.open()
self.left_arm_group.moveToJointPosition(self.left_joints,
self.preleft,
plan_only=False)
self.both_arms_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
else:
self.right_arm_group.moveToJointPosition(self.right_joints,
self.preright,
plan_only=False)
self.right_arm_group.moveToPose(self.pickgoal,
"right_gripper",
plan_only=False)
self.rightgripper.open()
self.right_arm_group.moveToJointPosition(self.right_joints,
self.preright,
plan_only=False)
self.both_arms_group.moveToJointPosition(self.both_arm_joints,
self.init_joint_angles,
plan_only=False)
def clasificar():
adaptative = True
pr_b = False
precision = 0.7
# Can free memory?
pub = rospy.Publisher("finished", String, queue_size=10)
# Initialize MoveIt! scene
p = PlanningSceneInterface("base")
arms_group = MoveGroupInterface("both_arms", "base")
rightarm_group = MoveGroupInterface("right_arm", "base")
leftarm_group = MoveGroupInterface("left_arm", "base")
# Create right arm instance
right_arm = baxter_interface.limb.Limb('right')
# Create right gripper instance
right_gripper = baxter_interface.Gripper('right')
right_gripper.calibrate()
right_gripper.open()
right_gripper.close()
offset_zero_point = 0.903
table_size_x = 0.714655654394
table_size_y = 1.05043717328
table_size_z = 0.729766045265
center_x = 0.457327827197
center_y = 0.145765166941
center_z = -0.538116977368
table_distance_from_gripper = -offset_zero_point + table_size_z + 0.0275 / 2
j = 0
k = 0
# Initialize object list
objlist = [
'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
'obj09', 'obj10', 'obj11'
]
p.clear()
p.attachBox('table',
table_size_x,
table_size_y,
table_size_z,
center_x,
center_y,
center_z,
'base',
touch_links=['pedestal'])
p.waitForSync()
# Move both arms to start state.
# Initial pos
rpos = PoseStamped()
rpos.header.frame_id = "base"
rpos.header.stamp = rospy.Time.now()
rpos.pose.position.x = 0.555
rpos.pose.position.y = 0.0
rpos.pose.position.z = 0.206
rpos.pose.orientation.x = 1.0
rpos.pose.orientation.y = 0.0
rpos.pose.orientation.z = 0.0
rpos.pose.orientation.w = 0.0
lpos = PoseStamped()
lpos.header.frame_id = "base"
lpos.header.stamp = rospy.Time.now()
lpos.pose.position.x = 0.65
lpos.pose.position.y = 0.6
lpos.pose.position.z = 0.206
lpos.pose.orientation.x = 1.0
lpos.pose.orientation.y = 0.0
lpos.pose.orientation.z = 0.0
lpos.pose.orientation.w = 0.0
while True:
# Move to initial position
rightarm_group.moveToPose(rpos,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
leftarm_group.moveToPose(lpos,
"left_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
# Get the middle point
locs = PoseArray()
locs = rospy.wait_for_message("clasificacion", PoseArray)
if (len(locs.poses) != 0):
punto_medio = PoseStamped()
punto_medio.header.frame_id = "base"
punto_medio.header.stamp = rospy.Time.now()
punto_medio.pose.position.x = locs.poses[0].position.x
punto_medio.pose.position.y = locs.poses[0].position.y
punto_medio.pose.position.z = locs.poses[0].position.z
punto_medio.pose.orientation.x = locs.poses[0].orientation.x
punto_medio.pose.orientation.y = locs.poses[0].orientation.y
punto_medio.pose.orientation.z = locs.poses[0].orientation.z
punto_medio.pose.orientation.w = locs.poses[0].orientation.w
alfa = 0.1
# Two parameters:
# When pr_b == 1, the program will try to adapt the trajectory for getting the precision established in the parameter of the same name
# When adaptative == 1, the program will try to get the best precision based on the precision of the last execution.
if (pr_b and not adaptative):
if precision >= 1:
punto_medio.pose.position.x += 0.01
else:
punto_medio.pose.position.x -= alfa * (1 - precision)
else:
print("If it is the first time executing, write -1")
precision_value = input()
if precision_value != -1.0:
punto_medio.pose.position.x += alfa * (1 - precision_value)
elif precision_value == 1.0:
adaptative = False
# Get the normal orientation for separating the objects
orient = (-1) * (1 / punto_medio.pose.orientation.x)
punto_medio.pose = rotate_pose_msg_by_euler_angles(
punto_medio.pose, 0.0, 0.0, orient)
rightarm_group.moveToPose(punto_medio,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
orient = 1.57 - orient
punto_medio.pose = rotate_pose_msg_by_euler_angles(
punto_medio.pose, 0.0, 0.0, orient)
rightarm_group.moveToPose(punto_medio,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
punto_medio.pose.position.x += 0.15
rightarm_group.moveToPose(punto_medio,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
time.sleep(1)
punto_medio.pose.position.x -= 0.3
rightarm_group.moveToPose(punto_medio,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
time.sleep(1)
rightarm_group.moveToPose(rpos,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
# Free the memory when finished
freemem = "free"
if not adaptative:
pub.publish(freemem)
else:
sys.exit("Cannot identify any object")
def dyn_wedge():
pub = rospy.Publisher("finished", String, queue_size=10)
# Initialize MoveIt scene
p = PlanningSceneInterface("base")
arms_group = MoveGroupInterface("both_arms", "base")
rightarm_group = MoveGroupInterface("right_arm", "base")
leftarm_group = MoveGroupInterface("left_arm", "base")
# Create right arm instance
right_arm = baxter_interface.limb.Limb('right')
# Create right gripper instance
right_gripper = baxter_interface.Gripper('right')
right_gripper.calibrate()
right_gripper.open()
right_gripper.close()
offset_zero_point=0.903
table_size_x = 0.714655654394
table_size_y = 1.05043717328
table_size_z = 0.729766045265
center_x = 0.457327827197
center_y = 0.145765166941
center_z = -0.538116977368
table_distance_from_gripper = -offset_zero_point+table_size_z+0.0275/2
j=0
k=0
# Initialize object list
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
p.clear()
p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
p.waitForSync()
# Move both arms to start state.
# Initial pose
rpos = PoseStamped()
rpos.header.frame_id = "base"
rpos.header.stamp = rospy.Time.now()
rpos.pose.position.x = 0.555
rpos.pose.position.y = 0.0
rpos.pose.position.z = 0.206 #0.18
rpos.pose.orientation.x = 1.0
rpos.pose.orientation.y = 0.0
rpos.pose.orientation.z = 0.0
rpos.pose.orientation.w = 0.0
lpos = PoseStamped()
lpos.header.frame_id = "base"
lpos.header.stamp = rospy.Time.now()
lpos.pose.position.x = 0.555
lpos.pose.position.y = 0.65
lpos.pose.position.z = 0.206#0.35
lpos.pose.orientation.x = 1.0
lpos.pose.orientation.y = 0.0
lpos.pose.orientation.z = 0.0
lpos.pose.orientation.w = 0.0
rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False)
# Get the middle point between the two centroids
locs = rospy.wait_for_message("clasificacion", PoseArray)
if(len(locs.poses)!=0):
punto_medio1 = PoseStamped()
punto_medio1.header.frame_id = "base"
punto_medio1.header.stamp = rospy.Time.now()
punto_medio1.pose.position.x = locs.poses[0].position.x
punto_medio1.pose.position.y = locs.poses[0].position.y
punto_medio1.pose.position.z = -0.08
punto_medio1.pose.orientation.x = locs.poses[0].orientation.x
punto_medio1.pose.orientation.y = locs.poses[0].orientation.y
punto_medio1.pose.orientation.z = locs.poses[0].orientation.z
punto_medio1.pose.orientation.w = locs.poses[0].orientation.w
else:
sys.exit("No se encuentran los puntos")
# Get the middle point again after a few seconds
tiempo = 3
time.sleep(tiempo)
locs = rospy.wait_for_message("clasificacion", PoseArray)
if(len(locs.poses)!=0):
punto_medio2 = PoseStamped()
punto_medio2.header.frame_id = "base"
punto_medio2.header.stamp = rospy.Time.now()
punto_medio2.pose.position.x = locs.poses[0].position.x
punto_medio2.pose.position.y = locs.poses[0].position.y
punto_medio2.pose.position.z = -0.08
punto_medio2.pose.orientation.x = locs.poses[0].orientation.x
punto_medio2.pose.orientation.y = locs.poses[0].orientation.y
punto_medio2.pose.orientation.z = locs.poses[0].orientation.z
punto_medio2.pose.orientation.w = locs.poses[0].orientation.w
else:
sys.exit("No se encuentran los puntos")
# Calculate speed of objects
vel = (punto_medio2.pose.position.y - punto_medio1.pose.position.y) / tiempo
# Predict position within a certain time
nuevo_tiempo = 2.1
posicion = nuevo_tiempo * vel * 2
if(posicion>=0):
nueva_y = punto_medio2.pose.position.y - posicion
else:
nueva_y = punto_medio2.pose.position.y + posicion
orientacion = (-1) * 1/punto_medio2.pose.orientation.x
new_or = 0.26
if orientacion > 0:
new_or = -0.26
# If there is time enough to sweep the objects
if vel > -0.08:
start = time.time()
punto_medio2.pose.position.y = nueva_y - nueva_y/2
punto_medio2.pose.position.x = punto_medio1.pose.position.x - 0.03
punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion - new_or)
rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
end = time.time()
tiempo = end - start
while(nuevo_tiempo - tiempo >= 1):
end = time.time()
nuevo_tiempo = end - start
else:
punto_medio2.pose.position.y = nueva_y
print(punto_medio2.pose.position.y)
punto_medio2.pose.position.x = punto_medio1.pose.position.x
punto_medio2.pose.position.y += 0.05
punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion)
rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
obj = punto_medio2.pose.position.y + 0.16
neg = 1
while punto_medio2.pose.position.y < obj:
punto_medio2.pose.position.y += 0.03
# Shake arm
punto_medio2.pose.position.x += neg * 0.09
rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
neg = (-1)*neg
time.sleep(2)
rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
freemem = "free"
pub.publish(freemem)
try:
t = listener.getLatestCommonTime(frame, "/map")
trans, rot = listener.lookupTransform(frame, "/map", t)
pos.append(trans)
ori.append(rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
break
if len(pos) == 8:
pos = np.array(pos)
acc = MotionPolicy(oldStates, pos, obstacle, goal)
oldStates = pos
acc = np.multiply(acc, ((rospy.get_time() - time)**2) / 2.0)
time = rospy.get_time()
poses = np.add(pos, acc)
pose = poses[-2]
rot = ori[-2]
gripper_pose = Pose(Point(pose[0], pose[1], pose[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = "/map"
gripper_pose_stamped.header.stamp = t
gripper_pose_stamped.pose = gripper_pose
move_group.moveToPose(gripper_pose_stamped, frames[-2][1:])
rate.sleep()
move_group.get_move_action().cancel_all_goals()
class screw_1(object):
def __init__(self):
# self.scene = PlanningSceneInterface("base_link")
self.dof = "7dof"
self.robot = moveit_commander.RobotCommander()
self._listener = tf.TransformListener()
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
if "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
# self.constrained_stow =[-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0]
self.constrained_stow = [
-2.037, 1.046, -2.877, -1.423, -1.143, 1.679, 1.690, 2.037,
-1.046, 2.877, 1.423, 1.143, -1.679, -1.690, 0.4, 0.288, 0
]
else:
rospy.logerr("DoF needs to be set at 7, aborting demo")
return
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
# self.scene.clear()
def goto_tuck(self):
# remove previous objects
raw_input("========== Press Enter to goto_tuck ========")
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints,
self.tucked,
0.05,
max_velocity_scaling_factor=0.4)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
raw_input("========== Press Enter to goto_plan_grasp ========")
while not rospy.is_shutdown():
try:
result = self.move_group.moveToJointPosition(
self._upper_body_joints,
self.constrained_stow,
0.05,
max_velocity_scaling_factor=0.3)
except:
continue
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def insert_bolt(self):
# right arm
# base_link y+
pose = self.curr_pose('right_ee_link')
pose.pose.position.y += 0.05
raw_input("=========== Press Enter to insert bolt ========")
# rospy.sleep(2.0)
while not rospy.is_shutdown():
try:
result = self.rmove_group.moveToPose(
pose,
"right_ee_link",
tolerance=0.02,
max_velocity_scaling_factor=0.1)
except:
continue
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def insert_nut(self):
pass
def start(self):
pass
def retreat(self):
pass
def curr_pose(self, eef):
pose = PoseStamped()
pose.header.frame_id = eef
curr_pose = self._listener.transformPose('/base_link', pose)
return curr_pose
def picknplace():
# Define initial parameters
p = PlanningSceneInterface("base")
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
# jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
# jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
# jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
# pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
# pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
# lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
# rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
# rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# Clear planning scene
# p.clear()
# # Add table as attached object
# p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# Move both arms to start state
# g.moveToJointPosition(jts_both, pos1, plan_only=False)
# # Get obj locations
# temp = rospy.wait_for_message("Dpos", PoseArray)
# locs = temp.poses
# locs_x = []
# locs_y = []
# orien = []
# size = []
# for i in range(len(locs)):
# locs_x.append(0.57 + locs[i].position.x)
# locs_y.append(-0.011 + locs[i].position.y)
# orien.append(locs[i].position.z*pi/180)
# size.append(locs[i].orientation.x)
# # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
# ind_rmv = []
# for i in range(0,len(locs)):
# if (locs_y[i] > 0.42):
# ind_rmv.append(i)
# continue
# for j in range(i,len(locs)):
# if not (i == j):
# if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
# ind_rmv.append(i)
# locs_x = del_meth(locs_x, ind_rmv)
# locs_y = del_meth(locs_y, ind_rmv)
# orien = del_meth(orien, ind_rmv)
# size = del_meth(size, ind_rmv)
# # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
# if locs_x:
# ig0 = itemgetter(0)
# sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
# locs_x = list(sorted_lists[1])
# locs_y = list(sorted_lists[2])
# orien = list(sorted_lists[3])
# size = list(sorted_lists[0])
# # Loop to continue pick and place until all objects are cleared from table
# j=0
# k=0
# while locs_x:
# p.clear() # CLear planning scene of all collision objects
# # Attach table as attached collision object to base of baxter
# p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# xn = locs_x[0]
# yn = locs_y[0]
# zn = -0.06
# thn = orien[0]
# sz = size[0]
# if thn > pi/4:
# thn = -1*(thn%(pi/4))
# # Add collision objects into planning scene
# objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
# for i in range(1,len(locs_x)):
# p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
# p.waitForSync()
# # Move both arms to pos2 (Right arm away and left arm on table)
# g.moveToJointPosition(jts_both, pos2, plan_only=False)
# Move left arm to pick object and pick object
# pickgoal = PoseStamped()
# pickgoal.header.frame_id = "base"
# pickgoal.header.stamp = rospy.Time.now()
# pickgoal.pose.position.x = xn
# pickgoal.pose.position.y = yn
# pickgoal.pose.position.z = zn+0.1
# pickgoal.pose.orientation.x = 1.0
# pickgoal.pose.orientation.y = 0.0
# pickgoal.pose.orientation.z = 0.0
# pickgoal.pose.orientation.w = 0.0
goal_pose = PoseStamped()
limb = baxter_interface.Limb('left')
gripper_pose = limb.endpoint_pose()
goal_pose.pose.position.x = gripper_pose['position'][0] #0.63
goal_pose.pose.position.y = gripper_pose['position'][1] #0.19519
goal_pose.pose.position.z = gripper_pose['position'][2] + 0.1 #0.05
goal_pose.pose.orientation.x = gripper_pose['orientation'][0]
goal_pose.pose.orientation.y = gripper_pose['orientation'][1]
goal_pose.pose.orientation.z = gripper_pose['orientation'][2]
goal_pose.pose.orientation.w = gripper_pose['orientation'][3]
gl.moveToPose(goal_pose, "left_gripper", plan_only=False)
def picknplace():
# Initialize the planning scene interface.
p = PlanningSceneInterface("base")
# Connect the arms to the move group.
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
# Create baxter_interface limb instance.
leftarm = baxter_interface.limb.Limb('left')
# Create baxter_interface gripper instance.
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
# Define the joints for the positions.
jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
pos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
lpos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024]
lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
placegoal = PoseStamped()
placegoal.header.frame_id = "base"
placegoal.header.stamp = rospy.Time.now()
placegoal.pose.position.x = 0.55
placegoal.pose.position.y = 0.28
placegoal.pose.position.z = 0
placegoal.pose.orientation.x = 1.0
placegoal.pose.orientation.y = 0.0
placegoal.pose.orientation.z = 0.0
placegoal.pose.orientation.w = 0.0
# Define variables.
offset_zero_point = 0.903
table_size_x = 0.714655654394
table_size_y = 1.05043717328
table_size_z = 0.729766045265
center_x = 0.457327827197
center_y = 0.145765166941
center_z = -0.538116977368
# The distance from the zero point in Moveit to the ground is 0.903 m.
# The value is not allways the same. (look in Readme)
center_z_cube= -offset_zero_point+table_size_z+0.0275/2
pressure_ok=0
j=0
k=0
start=1
locs_x = []
# Initialize a list for the objects and the stacked cubes.
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
boxlist= ['box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11']
# Clear planning scene.
p.clear()
# Add table as attached object.
p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
# Move both arms to start state where the right camera looks for objects.
g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False)
time.sleep(0.5)
# cProfile to measure the performance (time) of the task.
pr = cProfile.Profile()
pr.enable()
# Loop to continue pick and place until all objects are cleared from table.
while locs_x or start:
# Only for the start.
if start:
start = 0
# Receive the data from all objects from the topic "detected_objects".
temp = rospy.wait_for_message("detected_objects", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
# Adds the data from the objects.
for i in range(len(locs)):
locs_x.append(locs[i].position.x)
locs_y.append(locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected locations for same objects.
ind_rmv = []
for i in range(0,len(locs) ):
if (locs_y[i] > 0.24 or locs_x[i] > 0.75):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Do the task only if there are still objects on the table.
if locs_x:
# Clear planning scene.
p.clear()
# Add table as attached object.
p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
# Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Initialize the data of the biggest object on the table.
xn = locs_x[0]
yn = locs_y[0]
zn = -0.16
thn = orien[0]
sz = size[0]
if thn > pi/4:
thn = -1*(thn%(pi/4))
# Add the detected objects into the planning scene.
#for i in range(1,len(locs_x)):
#p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube)
# Add the stacked objects as collision objects into the planning scene to avoid moving against them.
#for e in range(0, k):
#p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes'])
if k>0:
p.attachBox(boxlist[0], 0.07, 0.07, 0.0275*k, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube, 'base', touch_links=['cubes'])
p.waitForSync()
# Move both arms to the second position.
g.moveToJointPosition(jts_both, pos2, max_velocity_scaling_factor = 1, plan_only=False)
# Initialize the pickgoal.
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
# Move to the approach pickgoal. (5 cm to pickgoal)
pickgoal.pose.position.z = zn+0.05
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
pickgoal.pose.position.z = zn
# Move to the pickgoal.
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.3, plan_only=False)
# Read the force in z direction.
b=leftarm.endpoint_effort()
z_= b['force']
z_force= z_.z
# Search again for objects, if the gripper isn't at the right position and presses on an object.
#print("----->force in z direction:", z_force)
if z_force>-4:
leftgripper.close()
attempts=0
pressure_ok=1
# If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
while(leftgripper.force()<25 and pressure_ok==1):
time.sleep(0.04)
attempts+=1
if(attempts>50):
leftgripper.open()
pressure_ok=0
print("----->pressure is to low<-----")
else:
print("----->gripper presses on an object<-----")
# Move back to the approach pickgoal.
pickgoal.pose.position.z = zn+0.05
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
# Move both arms to position 1.
g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False)
if pressure_ok and z_force>-4:
# Define the approach placegoal.
# Increase the height of the tower every time by 2.75 cm.
placegoal.pose.position.z =-0.145+(k*0.0275)+0.08
# Move to the approach pickgoal.
gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
# Define the placegoal.
placegoal.pose.position.z =-0.145+(k*0.0275)
gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
leftgripper.open()
k += 1
# Move to the approach placegoal.
placegoal.pose.position.z = -0.145+(k*0.0275)+0.08
gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
# Move left arm to start position pos1.
gl.moveToJointPosition(jts_left, lpos1, max_velocity_scaling_factor = 1, plan_only=False)
pr.disable()
sortby = 'cumulative'
ps=pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
p.clear()
Quaternion(-0.374, -0.701, 0.173, 0.635))
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
while not rospy.is_shutdown():
# Finish building the Pose_stamped message
# If the message stamp is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
gripper_pose_stamped.pose = pose
# Move gripper frame to the pose specified
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Hello there!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
class TrajectorySimulator():
def __init__(self):
# Instantiate a RobotCommander object.
# This object is an interface to the robot as a whole.
#robot = moveit_commander.RobotCommander()
# Instantiate a MoveGroupCommander object.
# This object is an interface to one 'group' of joints,
# in our case the joints of the arm.
self.group = MoveGroupInterface("arm", "base_link")
# Create DisplayTrajectory publisher to publish trajectories
# for RVIZ to visualize.
# self.display_trajectory_publisher = rospy.Publisher(
#"/move_group/display_planned_path",
#moveit_msgs.msg.DisplayTrajectory)
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(15)
print "============ Starting tutorial "
def get_state_info(self):
# Let's get some basic information about the robot.
# This info may be helpful for debugging.
# Get name of reference frame for this robot
print "============ Reference frame: %s" % self.group.get_planning_frame()
# Get name of end-effector link for this group
print "============ Reference frame: %s" % self.group.get_end_effector_link()
# List all groups defined on the robot
print "============ Robot Groups:"
print self.robot.get_group_names()
# Print entire state of the robot
print "============ Printing robot state"
print self.robot.get_current_state()
print "============"
def plan_to_pose_goal(self, goal):
### Planning to a Pose Goal
# Let's plan a motion for this group to a desired pose for the
# end-effector
print "============ Generating plan 1"
# Extract goal position
pose_x = goal[0]
pose_y = goal[1]
pose_z = goal[2]
# Extract goal orientation--provided in Euler angles
roll = goal[3]
pitch = goal[4]
yaw = goal[5]
# Convert Euler angles to quaternion, create PoseStamped message
quaternion = quaternion_from_euler(roll, pitch, yaw)
pose_target = PoseStamped()
pose_target.pose.position.x = pose_x
pose_target.pose.position.y = pose_y
pose_target.pose.position.z = pose_z
pose_target.pose.orientation.x = quaternion[0]
pose_target.pose.orientation.y = quaternion[1]
pose_target.pose.orientation.z = quaternion[2]
pose_target.pose.orientation.w = quaternion[3]
pose_target.header.frame_id = "base_link"
pose_target.header.stamp = rospy.Time.now()
print "============= PoseStamped message filled out"
# Execute motion
while not rospy.is_shutdown():
result = self.group.moveToPose(pose_target, "gripper_link", tolerance = 0.3, plan_only=False)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "================ Pose Achieved"
return
# Now we call the planner to compute the plan and visualize it if successful.
# We are just planning here, not asking move_group to actually move the robot.
print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(5)
print "============ Goal achieved"
# group.plan() automatically asks RVIZ to visualize the plan,
# so we need not explicitly ask RVIZ to do this.
# For completeness of understanding, however, we make this
# explicit request below. The same trajectory should be displayed
# a second time.
print "============ Visualizing plan1"
"""
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)
"""
def plan_to_jointspace_goal(self, pose):
# Let's set a joint-space goal and visualize it in RVIZ
# A joint-space goal differs from a pose goal in that
# a goal for the state of each joint is specified, rather
# than just a goal for the end-effector pose, with no
# goal specified for the rest of the arm.
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"]
while not rospy.is_shutdown():
result = self.group.moveToJointPosition(joints, pose, 0.1)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "============ Pose Achieved"
rospy.sleep(5)
return "Success"
class Grasping(object):
def __init__(self):
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.scene = PlanningSceneInterface("base_link")
self.move_group = MoveGroupInterface("arm", "base_link")
find_objects = "basic_grasping_perception/find_objects"
self.find_client = actionlib.SimpleActionClient(
find_objects, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def pickup(self, block, grasps):
rospy.sleep(1)
success, pick_result = self.pickplace.pick_with_retry(
block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
#swaps two blocks positions.
def swapBlockPos(self, block1Pos, block2Pos):
#intermediate point for movement
self.armIntermediatePose()
#intermediate positon on the table, used for short term storage of the first block manipulated in the sorting.
posIntermediate = np.array([0.725, 0])
self.armIntermediatePose()
# Get block to pick
while not rospy.is_shutdown():
rospy.loginfo("Picking object...")
self.updateScene()
cube, grasps = self.getGraspableObject(block1Pos)
if cube == None:
rospy.logwarn("Perception failed.")
continue
# Pick the block
if self.pickup(cube, grasps):
break
rospy.logwarn("Grasping failed.")
#self.tuck()
#Place the block
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = cube.primitive_poses[0]
pose.pose.position.z += 0.05
pose.header.frame_id = cube.header.frame_id
if self.place(cube, pose, posIntermediate):
break
rospy.logwarn("Placing failed.")
#place block 2 in block 1's
self.armIntermediatePose()
# Get block to pick
while not rospy.is_shutdown():
rospy.loginfo("Picking object...")
self.updateScene()
cube, grasps = self.getGraspableObject(block2Pos)
if cube == None:
rospy.logwarn("Perception failed.")
continue
# Pick the block
if self.pickup(cube, grasps):
break
rospy.logwarn("Grasping failed.")
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = cube.primitive_poses[0]
pose.pose.position.z += 0.05
pose.header.frame_id = cube.header.frame_id
if self.place(cube, pose, block1Pos):
break
rospy.logwarn("Placing failed.")
#place block1 in block 2's spot
self.armIntermediatePose()
while not rospy.is_shutdown():
rospy.loginfo("Picking object...")
self.updateScene()
cube, grasps = self.getGraspableObject(posIntermediate)
if cube == None:
rospy.logwarn("Perception failed.")
continue
# Pick the block
if self.pickup(cube, grasps):
break
rospy.logwarn("Grasping failed.")
#self.tuck()
self.armIntermediatePose()
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = cube.primitive_poses[0]
pose.pose.position.z += 0.05
pose.header.frame_id = cube.header.frame_id
if self.place(cube, pose, block2Pos):
break
rospy.logwarn("Placing failed.")
return
def place(self, block, pose_stamped, placePos):
rospy.sleep(1)
#creates a list of place positons for the block, with a specified x, y, and z.
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = "base_link"
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
#this x and y value are input as placePos through the function call.
l.place_pose.pose.position.x = placePos[0]
l.place_pose.pose.position.y = placePos[1]
places.append(copy.deepcopy(l))
#success = self.pickplace.place_with_retry(block.name, places, scene = self.scene)
#return success
## create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, scene=self.scene)
return success
def updateScene(self):
rospy.sleep(1)
# find objectsw
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
use_service=False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
use_service=True)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def getGraspableObject(self, pos):
graspable = None
for obj in self.objects:
if len(obj.grasps) < 1:
continue
if (obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[1] < 0.05 or \
obj.object.primitives[0].dimensions[1] > 0.07 or \
obj.object.primitives[0].dimensions[2] < 0.05 or \
obj.object.primitives[0].dimensions[2] > 0.07):
continue
if (obj.object.primitive_poses[0].position.z < 0.3) or \
(obj.object.primitive_poses[0].position.y > pos[1]+0.05) or \
(obj.object.primitive_poses[0].position.y < pos[1]-0.05) or \
(obj.object.primitive_poses[0].position.x > pos[0]+0.05) or \
(obj.object.primitive_poses[0].position.x < pos[0]-0.05):
continue
return obj.object, obj.grasps
return None, None
def armToXYZ(self, x, y, z):
rospy.sleep(1)
#new pose_stamped of the end effector that moves the arm out of the way of the vision for planning.
intermediatePose = PoseStamped()
intermediatePose.header.frame_id = 'base_link'
#position
intermediatePose.pose.position.x = x
intermediatePose.pose.position.y = y
intermediatePose.pose.position.z = z
#quaternion for the end position
intermediatePose.pose.orientation.w = 1
while not rospy.is_shutdown():
result = self.move_group.moveToPose(intermediatePose,
"wrist_roll_link")
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def armIntermediatePose(self):
self.armToXYZ(0.1, -0.7, 0.9)
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class TrajectoryGenerator:
# Wrapper class that facilitates trajectory planning.
# Includes methods for planning to arbitrary joint-space goals
# and end-effector pose goals.
def __init__(self):
self.group = MoveGroupInterface("arm", "base_link")
self.gripper = MoveGroupInterface("gripper", "base_link")
self.virtual_state = RobotState()
self.trajectory = None
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(5)
print "============ Starting planning"
def generate_trajectory(self, goal_poses):
for goal_pose in goal_poses:
if goal_pose.goal_type is "ee_pose":
traj_result = self.plan_to_ee_pose_goal(goal_pose.pose)
elif goal_pose.goal_type is "jointspace":
traj_result = self.plan_to_jointspace_goal(goal_pose.state)
elif goal_pose.goal_type is "gripper_posture":
traj_result = self.plan_to_gripper_goal(goal_pose.state)
else:
rospy.loginfo("Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'")
sys.exit()
approved_trajectories.append(traj_result)
return approved_trajectories
def plan_to_ee_pose_goal(self, goal):
# Extract goal position
pose_x = goal[0]
pose_y = goal[1]
pose_z = goal[2]
# Extract goal orientation--provided in Euler angles
roll = goal[3]
pitch = goal[4]
yaw = goal[5]
# Convert Euler angles to quaternion, create PoseStamped message
quaternion = quaternion_from_euler(roll, pitch, yaw)
pose_target = PoseStamped()
pose_target.pose.position.x = pose_x
pose_target.pose.position.y = pose_y
pose_target.pose.position.z = pose_z
pose_target.pose.orientation.x = quaternion[0]
pose_target.pose.orientation.y = quaternion[1]
pose_target.pose.orientation.z = quaternion[2]
pose_target.pose.orientation.w = quaternion[3]
pose_target.header.frame_id = "base_link"
pose_target.header.stamp = rospy.Time.now()
print "============= PoseStamped message filled out"
# Execute motion
while not rospy.is_shutdown():
result = self.group.moveToPose(
pose_target, "gripper_link", tolerance=0.02, plan_only=True, start_state=self.virtual_state
)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "================ Pose Achieved"
self.trajectory = result.planned_trajectory.joint_trajectory
return self.trajectory
def plan_to_jointspace_goal(self, pose):
joints = [
"shoulder_pan_joint",
"shoulder_lift_joint",
"upperarm_roll_joint",
"elbow_flex_joint",
"forearm_roll_joint",
"wrist_flex_joint",
"wrist_roll_joint",
]
while not rospy.is_shutdown():
result = self.group.moveToJointPosition(joints, pose, plan_only=True, start_state=self.virtual_state)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
print "============ Pose Achieved"
self.trajectory = result.planned_trajectory # .joint_trajectory
return self.trajectory
def plan_to_gripper_goal(self, posture):
joints = ["gripper_link", "l_gripper_finger_link", "r_gripper_finger_link"]
while not rospy.is_shutdown():
result = self.gripper.moveToJointPosition(joints, pose, plan_only=True, start_state=self.virtual_state)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Gripper Posture Plan Successful")
self.trajectory = result.planned_trajectory.joint_trajectory
return self.trajectory
def update_virtual_state(self):
# Sets joint names and sets position to final position of previous trajectory
if self.trajectory == None:
print "No trajectory available to provide a new virtual state."
print "Update can only occur after trajectory has been planned."
else:
self.virtual_state.joint_state.name = [
"shoulder_pan_joint",
"shoulder_lift_joint",
"upperarm_roll_joint",
"elbow_flex_joint",
"forearm_roll_joint",
"wrist_flex_joint",
"wrist_roll_joint",
]
self.virtual_state.joint_state.position = self.trajectory.points[-1].positions
def clear_virtual_state(self):
self.virtual_state.joint_state.joint_names = []
self.virtual_state.joint_state.position = []
def execute_trajectory(self, trajectory):
execute_known_trajectory = rospy.ServiceProxy("execute_known_trajectory", ExecuteKnownTrajectory)
result = execute_known_trajectory(trajectory)
def picknplace():
# Define initial parameters
p = PlanningSceneInterface("base")
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# Clear planning scene
p.clear()
# Add table as attached object
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# Move both arms to start state
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Loop to continue pick and place until all objects are cleared from table
j=0
k=0
while locs_x:
p.clear() # CLear planning scene of all collision objects
# Attach table as attached collision object to base of baxter
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
xn = locs_x[0]
yn = locs_y[0]
zn = -0.06
thn = orien[0]
sz = size[0]
if thn > pi/4:
thn = -1*(thn%(pi/4))
# Add collision objects into planning scene
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
for i in range(1,len(locs_x)):
p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
p.waitForSync()
# Move both arms to pos2 (Right arm away and left arm on table)
g.moveToJointPosition(jts_both, pos2, plan_only=False)
# Move left arm to pick object and pick object
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn+0.1
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles
pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
pickgoal.pose.position.z = zn
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
leftgripper.close()
pickgoal.pose.position.z = zn+0.1
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Move both arms to pos1
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Place object
stleft = PoseStamped()
stleft.header.frame_id = "base"
stleft.header.stamp = rospy.Time.now()
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.55
stleft.pose.position.z = 0.1
stleft.pose.orientation.x = 1.0
stleft.pose.orientation.y = 0.0
stleft.pose.orientation.z = 0.0
stleft.pose.orientation.w = 0.0
# Stack objects (large cubes) if size if greater than some threshold
if sz > 16.:
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.7
stleft.pose.position.z = -0.04+(k*0.05)
k += 1
gl.moveToPose(stleft, "left_gripper", plan_only=False)
leftgripper.open()
stleft.pose.position.z = 0.3
gl.moveToPose(stleft, "left_gripper", plan_only=False)
class FetchClient(object):
def __init__(self):
self.bfp = True
self.cx = 320
self.cy = 240
self.ax = 57.5
self.ay = 45
self.fx = self.cx / math.tan(self.ax * math.pi / 180.0 / 2)
self.fy = self.cy / math.tan(self.ay * math.pi / 180.0 / 2)
self.robot = robot_interface.Robot_Interface()
self.br = tf.TransformBroadcaster()
self.move_group = MoveGroupInterface("arm", "base_link")
self.pose_group = moveit_commander.MoveGroupCommander("arm")
self.cam = camera.RGBD()
while True:
try:
cam_info = self.cam.read_info_data()
if cam_info is not None:
break
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.pcm.fromCameraInfo(cam_info)
def broadcast_position(self, position, name):
pass
def calc_3D_position(self, u, v):
dep_data = self.robot.get_img_data()[1]
z = self.robot.get_depth((u, v), dep_data)
if z == 0:
return None, None, None
x = (u - self.cx) * z / self.fx
y = (v - self.cy) * z / self.fy
return x, y, z
def broadcast_from_pix(self, point, name):
while self.bfp:
# dep = self.robot.get_depth(point, dep_data)
# while dep == 0:
# print("depth info error")
# dep = self.robot.get_depth(point, dep_data)
#
# td_points = self.pcm.projectPixelTo3dRay(point)
# norm_pose = np.array(td_points)
# norm_pose = norm_pose / norm_pose[2]
# norm_pose = norm_pose * (dep)
norm_pose = self.calc_3D_position(point[0], point[1])
a = tf.transformations.quaternion_from_euler(ai=-2.355,
aj=-3.14,
ak=0.0)
b = tf.transformations.quaternion_from_euler(ai=0.0,
aj=0.0,
ak=1.57)
base_rot = tf.transformations.quaternion_multiply(a, b)
self.br.sendTransform(norm_pose, base_rot, rospy.Time.now(), name,
'head_camera_rgb_optical_frame')
def get_position_of_baselink(self, name):
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
pose, rot = listener.lookupTransform('base_link', name,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
rate.sleep()
if pose:
# rot = tf.transformations.quaternion_from_euler(0*DEG_TO_RAD, 90*DEG_TO_RAD, 0*DEG_TO_RAD)
return pose
def create_tf_of_base_link(self, name, position, rot):
quat = tf.transformations.quaternion_from_euler(ai=rot[0],
aj=rot[1],
ak=rot[2])
while True:
self.br.sendTransform(position, quat, rospy.Time.now(), name,
'base_link')
def show_image_with_point(self, im):
plt.imshow(im)
pylab.show()
def reach(self, point_xyz):
pose = Pose(Point(point_xyz[0], point_xyz[1], point_xyz[2]),
Quaternion(0, 0.707106781187, 0, 0.707106781187))
# pose = Pose(Point(0.57503179279, 0.170263536187, 0.97855338914), Quaternion(0, 0.707106781187, 0, 0.707106781187))
print(pose)
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
gripper_pose_stamped.header.stamp = rospy.Time.now()
gripper_pose_stamped.pose = pose
gripper_frame = 'wrist_roll_link'
# gripper_frame = 'l_gripper_finger_link'
self.move_group.moveToPose(gripper_pose_stamped,
gripper_frame,
plan_only=True)
result = self.move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
print("plan0 success!")
height = result.planned_trajectory.joint_trajectory.points[0]
gripper_pose_stamped.pose.position.z += height
self.move_group.moveToPose(gripper_pose_stamped,
gripper_frame,
plan_only=True)
result1 = self.move_group.get_move_action().get_result()
if result1:
if result1.error_code.val == MoveItErrorCodes.SUCCESS:
print("plan1 success!")
joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint",
"forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
joint_pose = result1.planned_trajectory.joint_trajectory[
0].position[6:13]
print joint_pose
self.move_group.moveToJointPosition(joint_names,
joint_pose,
wait=False)
result2 = self.move_group.get_move_action().get_result()
if result2:
# Checking the MoveItErrorCode
if result2.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("reach!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("reach fail")
else:
rospy.logerr("MoveIt! failure no result returned.")
else:
rospy.logerr("plan1 fail")
else:
rospy.logerr("MoveIt! failure no result returned.")
else:
rospy.logerr("MoveIt! failure no result returned.")
