class GripperClient:
def __init__(self):
self.move_group = MoveGroupInterface("gripper", "base_link")
self.joint_names = ['l_gripper_finger_joint', 'r_gripper_finger_joint']
def open_gripper(self):
value = [0.1, 0.1]
self.__execu(value)
# Plans the joints in joint_names to angles in pose
def close_gripper(self):
value = [0.0, 0.0]
self.__execu(value)
# Plans the joints in joint_names to angles in pose
def __execu(self, value):
self.move_group.moveToJointPosition(self.joint_names,
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:
print 'gripper'
else:
# If you get to this point please search for:
rospy.logerr(
"Arm goal in state: %s",
self.move_group.get_move_action().get_joint_values())
else:
rospy.logerr("MoveIt! failure no result returned.")
def gotoInit(self):
move_group = MoveGroupInterface("arm", "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
# Plans the joints in joint_names to angles in pose
init = self.trajActionGoal.trajectory.points[0].positions
move_group.moveToJointPosition(joint_names, init, wait=False)
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("Moved to Init")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
class moveRobot(object):
def __init__(self):
self.move_delay = 0.2
self.lastpose = []
self.lasttime = time.time()
self.need_plan = True
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"
]
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
def callback(self, data):
if data.angles != self.lastpose:
self.lastpose = data.angles
self.lasttime = time.time()
self.need_plan = True
def check_pose_updates(self):
if (not (self.need_plan
and time.time() - self.lasttime > self.move_delay)):
return
self.need_plan = False
pose = [0, 0, 0, 0, 0, 0, 0, 0]
pose_data = self.lastpose
for i in range(len(pose_data)):
pose[i] = pose_data[i].data
rospy.loginfo(rospy.get_caller_id() + " I heard %s", pose)
self.move_group.moveToJointPosition(self.joint_names, pose, wait=True)
print 'Moved To New Pose'
def main():
mgi = MoveGroupInterface("l_arm", "benderbase_link", None,
False) #"bender/l_shoulder_pitch_link"
tip_link = "benderl_wrist_pitch_link"
joint_names = [
'l_shoulder_pitch_joint', 'l_shoulder_roll_joint',
'l_shoulder_yaw_joint', 'l_elbow_pitch_joint', 'l_elbow_yaw_joint',
'l_wrist_pitch_joint'
]
rospy.loginfo('STARTING TEST')
count = 0
succ_plan = 0
home = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
mgi.moveToJointPosition(joint_names, home)
rospy.loginfo('POSITION: home')
rospy.sleep(2.0)
#---------------------------------------------------------------------------------------------------------------------------------------------------
goal = PoseStamped()
#Position
goal.pose.position.x, goal.pose.position.y, goal.pose.position.z = 0.2, 0.1, 0.1
#simple_orientation = Quaternion(0.0,-0.70710678,0.0,0.70710678)
#Orientation
goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w = -0.0119682, -0.70163, -0.00910682, 0.712382 #-0.0148,-0.733 , 0.0, 0.678
goal.header.frame_id = "benderbase_link"
#---------------------------------------------------------------------------------------------------------------------------------------------------
#------------------------------------------------Iterarion Params-----------------------------------------------------------------------------------
dl = 0.03 #precision en metros (0.03)
Xmin = -0.623244 #-0.05
Xmax = 0.615229 #0.8
Ymin = -0.829381 #-0.4
Ymax = -0.261919 #0.05
Zmin = 0.393434 #0.55
Zmax = 1.346654 #0.85
# ~17 horas worst case scenario
#---------------------------------------------------------------------------------------------------------------------------------------------------
workspace = [] #matrix for saving coordinates and error codes
for i in my_range(Xmin, Xmax, dl):
for j in my_range(Ymin, Ymax, dl):
for k in my_range(Zmin, Zmax, dl):
goal.pose.position.x, goal.pose.position.y, goal.pose.position.z = i, j, k
error_code = Move(mgi, tip_link, goal)
if (error_code == 1): #Move(mgi,tip_link,goal)==1):
succ_plan += 1
count += 1
workspace.append([error_code, i, j, k])
percentage = (succ_plan / count) * 100
rospy.loginfo('succeeded plannings: {0}/{1} ({2}%)'.format(
succ_plan, count, percentage))
rospy.loginfo('TEST ENDED')
with open(
'/home/robotica/uchile_ws/pkgs/base_ws/bender_core/bender_test_config_v38/scripts/workspace.csv',
'wb') as f:
writer = csv.writer(f)
writer.writerows(workspace)
def arm_move(self):
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
#planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_left_ground", 1, 1.5,
self.translation[2][0], self.translation[2][1],
self.translation[2][2])
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TF joint names
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
# Lists of joint angles in the same order as in joint_names
disco_poses = [[0, 2.5, -0.1, 3.0, 1.5, 3.0, 1.0, 3.0]]
for pose in disco_poses:
if rospy.is_shutdown():
break
# Plans the joints in joint_names to angles in pose
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Disco!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
class ArmController(object):
def __init__(self):
# Create move group interface for a fetch robot
self._move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
self._planning_scene = PlanningSceneInterface("base_link")
self._planning_scene.removeCollisionObject("my_front_ground")
self._planning_scene.removeCollisionObject("my_back_ground")
self._planning_scene.removeCollisionObject("my_right_ground")
self._planning_scene.removeCollisionObject("my_left_ground")
self._planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self._planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self._planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self._planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def SetPose(self, target_pose):
# Plans the joints in joint_names to angles in pose
self._move_group.moveToJointPosition(target_pose.keys(),
target_pose.values(),
wait=False)
# Since we passed in wait=False above we need to wait here
self._move_group.get_move_action().wait_for_result()
result = self._move_group.get_move_action().get_result()
return_result = False
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("MoveIt pose reached.")
return_result = True
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self._move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
self._move_group.get_move_action().cancel_all_goals()
return return_result
def SetPoseWithRetry(self, target_pose, max_time=3):
for _ in range(max_time):
if self.SetPose(target_pose):
return True
return False
class Arm:
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6)
def MoveToPose(self, X, Y, Z, Roll, Pitch, Yaw):
orientation = tf.transformations.quaternion_from_euler(
Roll, Pitch, Yaw)
self.poseStamped = PoseStamped()
self.poseStamped.header.frame_id = 'base_link'
self.poseStamped.header.stamp = rospy.Time.now()
self.poseStamped.pose.position.x = X
self.poseStamped.pose.position.y = Y
self.poseStamped.pose.position.z = Z
self.poseStamped.pose.orientation.x = orientation[0]
self.poseStamped.pose.orientation.y = orientation[1]
self.poseStamped.pose.orientation.z = orientation[2]
self.poseStamped.pose.orientation.w = orientation[3]
self.moveGroup.moveToPose(self.poseStamped,
'gripper_link',
0.005,
max_velocity_scaling_factor=0.2)
self.result = self.moveGroup.get_move_action().get_result()
def Tuck(self):
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
self.moveGroup.moveToJointPosition(joints,
pose,
0.01,
max_velocity_scaling_factor=0.2)
while not rospy.is_shutdown():
self.result = self.moveGroup.get_move_action().get_result()
if self.result.error_code.val == MoveItErrorCodes.SUCCESS:
break
def AddDice(self, name, X, Y, Z):
self.planning_scene.addCube(name, 0.1, X, Y, Z)
def RemoveDice(self, name):
self.planning_scene.removeCollisionObject(name)
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface("manipulator","base_link") #MoveGroupInterface のインスタンスの作成
planning_scene = PlanningSceneInterface("base_link") #PlanningSceneInterface のインスタンスの作成
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす
move_group.get_move_action().wait_for_result() #wait result
result = move_group.get_move_action().get_result() #result を代入
move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
def set_init_pose(sefl):
#set init pose
move_group = MoveGroupInterface("manipulator","base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint",
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
move_group.get_move_action().cancel_all_goals()
class GraspingClient(object):
def __init__(self):
self.move_group = MoveGroupInterface("arm", "base_link")
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(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, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def straight(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [0, 0, 0, 0, 0, 0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def callback_posegoal(data):
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"]
move_group = MoveGroupInterface("arm_with_torso", "base_link")
pose = [0,0,0,0,0,0,0,0]
pose_data = data.angles
for i in range(len(pose_data)):
pose[i] = pose_data[i].data
rospy.loginfo(rospy.get_caller_id() + " I heard %s", pose)
move_group.moveToJointPosition(joint_names,pose,wait=True)
print ('Moved To New Pose')
def tuck():
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
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]
while not rospy.is_shutdown():
result = client.moveToJointPosition(joints,
pose,
0.0,
max_velocity_scaling_factor=0.5)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
move_thread.stop()
return
def _home_arm_planner(self):
if self._prefix == 'left':
rospy.sleep(5)
else:
move_group_jtas = MoveGroupInterface("upper_body", "base_link")
move_group_jtas.setPlannerId("RRTConnectkConfigDefault")
success = False
while not rospy.is_shutdown() and not success:
result = move_group_jtas.moveToJointPosition(
self._body_joints, self._homed, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.logerr("_home_arm_planner completed ")
success = True
else:
rospy.logerr(
"_home_arm_planner: _home_arm_planner failed (%d)" %
result.error_code.val)
self._arms_homing = True
self._ctl.api.MoveHome()
self._ctl.api.InitFingers()
self.home_arm_pub.publish(Bool(True))
rospy.sleep(2.0)
self._arms_homing = False
self._planner_homing = False
class MovoUpperBody:
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"
]
homed = [
-1.5,-0.2,-0.175,-2.0,2.0,-1.24,-1.1, \
1.5,0.2,0.15,2.0,-2.0,1.24,1.1,\
0.35,0,0
]
tucked = [
-1.6,-1.4,0.4,-2.7,0.0,0.5,-1.7,\
1.6,1.4,-0.4,2.7,0.0,-0.5, 1.7,
0.04, 0, 0]
sidearms = [
-0.3 , -1.7 ,0.0 ,-1.7 ,1.8 ,-1.8 ,-1.1, \
0.3 , 1.7 ,0.0 ,1.7 ,-1.8 ,1.8 ,1.1 ,\
.35, 0, -1]
gripper_closed = 0.01
gripper_open = 0.165
def __init__(self):
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lgripper = GripperActionClient('left')
self.rgripper = GripperActionClient('right')
def move(self, pose):
success = False
while not rospy.is_shutdown() and not success:
result = self.move_group.moveToJointPosition(\
self.upper_body_joints, pose, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
success = True
else:
rospy.logerr("moveToJointPosition failed (%d)"\
%result.error_code.val)
def untuck(self):
rospy.loginfo("untucking the arms")
self.lgripper.command(self.gripper_open)
self.rgripper.command(self.gripper_open)
self.move(self.homed)
def tuck(self):
self.move(self.tucked)
self.lgripper.command(self.gripper_closed)
self.rgripper.command(self.gripper_closed)
def side(self):
self.move(self.sidearms)
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
rospy.loginfo("Add collision objects")
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.removeCollisionObject("box_1")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.scene.addBox("box_1", 0.44, 0.43, 0.68, 0.7, -0.4, .34)
self.scene.waitForSync()
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class TuckThread(Thread):
def __init__(self, untuck=False):
Thread.__init__(self)
self.client = None
self.untuck = untuck
self.start()
def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
self.client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
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"]
if not self.untuck:
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
else:
pose = [0.05, 0.29, -0.60, -0.51, 1.45, 0.52, 0.88, -1.97]
while not rospy.is_shutdown():
result = self.client.moveToJointPosition(joints,
pose,
0.0,
max_velocity_scaling_factor=0.5)
if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
if move_thread:
move_thread.stop()
# On success quit
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("done")
sys.exit(0)
return
def stop(self):
if self.client:
self.client.get_move_action().cancel_goal()
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("failed")
sys.exit(0)
def tuck():
move_group = MoveGroupInterface("arm", "base_link")
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 = move_group.moveToJointPosition(joints, pose, 0.02)
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
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'
]
poses, pose_name = get_pose(args.case, args.reset)
for pose in poses:
if rospy.is_shutdown():
break
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
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 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"
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface("arm_with_torso", "base_link")
#set arm initial position
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
#pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pose = [0.385, -1.0, -1.0, 0.0, 1.0, 0.0, 1.5, 0.0]
#pose = [0.385, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
result = move_group.moveToJointPosition(joints, pose, 0.02)
class TuckThread(Thread):
def __init__(self):
Thread.__init__(self)
self.client = None
self.start()
def run(self):
move_thread = None
if not is_moveit_running():
rospy.loginfo("starting moveit")
move_thread = MoveItThread()
rospy.loginfo("Waiting for MoveIt...")
self.client = MoveGroupInterface("arm_with_torso", "base_link")
rospy.loginfo("...connected")
# Padding does not work (especially for self collisions)
# So we are adding a box above the base of the robot
scene = PlanningSceneInterface("base_link")
scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
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]
while not rospy.is_shutdown():
result = self.client.moveToJointPosition(joints,
pose,
0.0,
max_velocity_scaling_factor=0.5)
if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
scene.removeCollisionObject("keepout")
if move_thread:
move_thread.stop()
# On success quit
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("done")
sys.exit(0)
return
def stop(self):
if self.client:
self.client.get_move_action().cancel_goal()
# Stopping the MoveIt thread works, however, the action client
# does not get shut down, and future tucks will not work.
# As a work-around, we die and roslaunch will respawn us.
rospy.signal_shutdown("failed")
sys.exit(0)
def move():
rospy.init_node("move")
move_group = MoveGroupInterface("arm_with_torso", "base_link")
#planning scene setup
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
#move head camera
client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
rospy.loginfo("Waiting for head_controller...")
client.wait_for_server()
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = "base_link"
goal.target.point.x = 1.2
goal.target.point.y = 0.0
goal.target.point.z = 0.0
goal.min_duration = rospy.Duration(1.0)
client.send_goal(goal)
client.wait_for_result()
#arm setup
move_group = MoveGroupInterface("arm", "base_link")
#set arm initial position
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
if __name__ == '__main__':
try:
global objectName, closeRC, closeLC
object_topic = rospy.get_param('~object_topic')
rospy.Subscriber(object_topic, obj, addObj)
# cProfile to measure the performance (time) of the task.
pr = cProfile.Profile()
pr.enable()
rightgripper.calibrate()
leftgripper.calibrate()
rightgripper.open()
leftgripper.open()
g.moveToJointPosition(jts_both,
pos_high,
max_velocity_scaling_factor=0.5,
plan_only=False)
# Awaits for the planning scene to build correctly
time.sleep(10)
print "There is", len(objectName), "object(s) detected on the table"
# Sort the object list
objectName, collisionObjects = sortObjects(objectName)
# Pick and place loop
i = 0
for collisionObject in collisionObjects:
name = objectName[i]
if collisionObject.primitive_poses[0].position.y < 0:
side = 'right'
else:
side = 'left'
print "READY TO PICK UP OBJECT", name
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
obj_cts = 0
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
print " find goal"
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
print " find client"
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
print "the object %s should be removed" %(name)
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
print "the object %s should be removed" %(name)
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
print "-----------------------"
print obj.object.primitive_poses[0]
print " x: %d" %(obj.object.primitive_poses[0].position.x)
# if obj.object.primitive_poses[0].position.y > 0.0
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
use_service = False)
# def addSolidPrimitive (self, name, solid, pose, use_service=True)
print "1, %d" %(idx)
if obj.object.primitive_poses[0].position.x < 1.25:
objects.append([obj, obj.object.primitive_poses[0].position.z])
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
print "2,"
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
# store graspable objects by Z
objects.sort(key=lambda object: object[1])
objects.reverse()
self.objects = [object[0] for object in objects]
rospy.loginfo("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
rospy.loginfo("number of objects...:::::::" + str(len(objects)))
#for object in objects:
# print(object[0].object.name, object[1])
#exit(-1)
def getGraspableObject(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
rospy.loginfo("must more than one object")
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25:
continue
# has to located in +y
if obj.object.primitive_poses[0].position.y < 0.0:
print "has to located in +y"
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
print "z has to larger than 0.5 "
continue
rospy.loginfo("object pose:")
print obj.object.primitive_poses[0], obj.object.primitives[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
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
def place(self, block, pose_stamped):
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.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
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.place_with_retry(block.name,
places,
scene=self.scene)
return success
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(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, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def 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 FetchRobotApi:
def __init__(self):
self.moving_mode = False
self.waldo_mode = False
self.prev_joy_buttons = 0
# See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
self.joint_names = [
#'torso_lift_joint',
#'bellows_joint',
#'head_pan_joint',
#'head_tilt_joint',
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
'l_gripper_finger_joint',
#'r_gripper_finger_joint',
]
self.joint_name_map = dict([(name, index) for index, name in enumerate(self.joint_names)])
self.cur_joint_pos = np.zeros([len(self.joint_names)])
self.cur_joint_vel = np.zeros([len(self.joint_names)])
self.cur_joint_effort = np.zeros([len(self.joint_names)])
self.cur_base_scan = np.zeros([662], dtype=np.float32)
self.cur_head_camera_depth_image = np.zeros([480,640], dtype=np.float32)
self.cur_head_camera_rgb_image = np.zeros([480,640,3], dtype=np.uint8)
self.timeseq = None
self.timeseq_mutex = threading.Lock()
self.image_queue = Queue.Queue()
self.image_compress_thread = threading.Thread(target=self.image_compress_main)
self.image_compress_thread.daemon = True
self.image_compress_thread.start()
logger.info('Subscribing...')
self.subs = []
if 1: self.subs.append(rospy.Subscriber('/joint_states', JointState, self.joint_states_cb))
if 0: self.subs.append(rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
if 1: self.subs.append(rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image), self.head_camera_depth_image_cb))
if 1: self.subs.append(rospy.Subscriber('/head_camera/rgb/image_raw', numpy_msg(Image), self.head_camera_rgb_image_raw_cb))
if 1: self.subs.append(rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
logger.info('Subscribed')
self.arm_effort_pub = rospy.Publisher('/arm_controller/weightless_torque/command', JointTrajectory, queue_size=2)
#self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
self.gripper_client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher('/arm_controller/cartesian_twist/command', Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True)
self.arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
self.arm_trajectory_client.wait_for_server()
if 0:
logger.info('Creating MoveGroupInterface...')
self.move_group = MoveGroupInterface('arm_with_torso', 'base_link', plan_only = True)
logger.info('Created MoveGroupInterface')
if 0:
logger.info('Creating PlanningSceneInterface...')
self.planning_scene = PlanningSceneInterface('base_link')
self.planning_scene.removeCollisionObject('my_front_ground')
self.planning_scene.removeCollisionObject('my_back_ground')
self.planning_scene.removeCollisionObject('my_right_ground')
self.planning_scene.removeCollisionObject('my_left_ground')
self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2, -1.0)
logger.warn('FetchRobotGymEnv ROS node running')
self.head_point_client.wait_for_server()
logger.warn('FetchRobotGymEnv ROS node connected')
def start_timeseq(self, script):
timeseq = TimeseqWriter.create(script)
timeseq.add_channel('robot_joints', 'FetchRobotJoints')
timeseq.add_channel('gripper_joints', 'FetchGripperJoints')
timeseq.add_schema('FetchRobotJoints',
('torso_lift_joint', 'JointState'),
('head_pan_joint', 'JointState'),
('head_tilt_joint', 'JointState'),
('shoulder_pan_joint', 'JointState'),
('shoulder_lift_joint', 'JointState'),
('upperarm_roll_joint', 'JointState'),
('elbow_flex_joint', 'JointState'),
('forearm_roll_joint', 'JointState'),
('wrist_flex_joint', 'JointState'),
('wrist_roll_joint', 'JointState'),
)
timeseq.add_schema('FetchGripperJoints',
('l_gripper_finger_joint', 'JointState'),
)
timeseq.add_schema('JointState',
('pos', 'double'),
('vel', 'double'),
('effort', 'double'),
)
timeseq.add_channel('arm_action', 'FetchArmWeightlessTorqueAction')
timeseq.add_schema('FetchArmWeightlessTorqueAction',
('shoulder_pan_joint', 'WeightlessTorqueAction'),
('shoulder_lift_joint', 'WeightlessTorqueAction'),
('upperarm_roll_joint', 'WeightlessTorqueAction'),
('elbow_flex_joint', 'WeightlessTorqueAction'),
('forearm_roll_joint', 'WeightlessTorqueAction'),
('wrist_flex_joint', 'WeightlessTorqueAction'),
('wrist_roll_joint', 'WeightlessTorqueAction'),
)
timeseq.add_schema('WeightlessTorqueAction',
('action', 'double'),
('effort', 'double'),
)
timeseq.add_channel('gripper_action', 'FetchGripperAction')
timeseq.add_schema('FetchGripperAction',
('action', 'double'),
('effort', 'double'),
('pos', 'double'),
)
timeseq.add_channel('head_camera_rgb', 'VideoFrame')
timeseq.add_schema('VideoFrame',
('url', 'string'),
)
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.close()
self.timeseq = timeseq
def close_timeseq(self):
with self.timeseq_mutex:
if self.timeseq is not None:
self.timeseq.close()
threading.Thread(target=self.timeseq.upload_s3).start()
self.timeseq = None
def start_axis_video(self):
cameras = rospy.get_param('/axis_cameras')
if cameras and len(cameras):
with self.timeseq_mutex:
if self.timeseq:
for name, info in cameras.items():
logging.info('Camera %s: %s', name, repr(info))
self.timeseq.start_axis_video(timeseq_name = name,
auth_header=info.get('auth_header'),
daemon_endpoint=info.get('daemon_endpoint'),
ipaddr=info.get('ipaddr'),
local_link_prefix=info.get('local_link_prefix'),
remote_traces_dir=info.get('remote_traces_dir'),
resolution=info.get('resolution'))
def stop_axis_video(self):
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.stop_axis_video()
def base_scan_cb(self, data):
# fmin replaces nans with 15
self.cur_base_scan = np.fmin(np.array(data.ranges), 15.0)
def head_camera_depth_image_cb(self, data):
shape = [data.height, data.width]
dtype = np.dtype(np.float32)
npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
npdata.strides = (data.step, dtype.itemsize)
self.cur_head_camera_depth_image = np.fmin(npdata, 5.0)
def head_camera_rgb_image_raw_cb(self, data):
shape = [data.height, data.width, 3]
dtype = np.dtype(np.uint8)
npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
npdata.strides = (data.step, dtype.itemsize*3, 1)
self.cur_head_camera_rgb_image = npdata
if self.timeseq:
self.image_queue.put(('head_camera_rgb', data))
def joint_states_cb(self, data):
"""
Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping.
"""
t0 = time.time()
for i in range(len(data.name)):
name = data.name[i]
jni = self.joint_name_map.get(name, -1)
if jni >= 0:
self.cur_joint_pos[jni] = data.position[i]
self.cur_joint_vel[jni] = data.velocity[i]
self.cur_joint_effort[jni] = data.effort[i]
with self.timeseq_mutex:
if self.timeseq:
channel, channel_type = ((data.name[0] == 'l_wheel_joint' and ('robot_joints', 'FetchRobotJoints')) or
(data.name[0] == 'l_gripper_finger_joint' and ('gripper_joints', 'FetchGripperJoints')) or
(None, None))
if channel:
state = dict([(jn, {
'__type': 'JointState',
'pos': data.position[i],
'vel': data.velocity[i],
'effort': data.effort[i],
}) for i, jn in enumerate(data.name)])
state['__type'] = channel_type
state['rxtime'] = t0
self.timeseq.add(data.header.stamp.to_sec(), channel, state)
if 0:
t1 = time.time()
print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % (t0, t1-t0, data.header.stamp.to_sec(), data.header.stamp.to_sec()-t0, channel)
def image_compress_main(self):
while True:
channel, data = self.image_queue.get()
if 0: print 'len(data.data)=%d data.width=%d data.height=%d' % (len(data.data), data.width, data.height)
im = PIL.Image.frombytes('RGB', (data.width, data.height), data.data, 'raw')
jpeg_writer = StringIO()
im.save(jpeg_writer, 'jpeg')
im_url = 'data:image/jpeg;base64,' + base64.b64encode(jpeg_writer.getvalue())
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.add(data.header.stamp.to_sec(), channel, {
'__type': 'VideoFrame',
'url': im_url,
})
def move_to_start(self):
self.moving_mode = True
# Look down
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = '/base_link'
goal.target.point.x = 1.5
goal.target.point.y = 0.0
goal.target.point.z = -0.2
goal.min_duration = rospy.Duration(0.5)
if 0: logger.info('Point head to %s...', goal);
self.head_point_client.send_goal(goal)
if 0: logger.info('Point head sent')
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
joints = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
]
pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if 0: logger.info('Got trajectory %s', result.planned_trajectory)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = result.planned_trajectory.joint_trajectory
logger.info('sending trajectory to arm...')
self.arm_trajectory_client.send_goal(follow_goal)
result = self.arm_trajectory_client.wait_for_result()
logger.info('arm followed trajectory %s', result)
else:
logger.warn('moveToJointPosition returned %s', result)
return
result = self.head_point_client.wait_for_result()
logger.info('head followed trajectory %s', result)
logger.info('sending empty arm goal')
empty_goal = FollowJointTrajectoryGoal()
self.arm_trajectory_client.send_goal(empty_goal)
logger.info('Point head done')
self.moving_mode = False
def set_arm_action(self, action):
arm_joints = [
('shoulder_pan_joint', 1.57, 33.82),
('shoulder_lift_joint', 1.57, 131.76),
('upperarm_roll_joint', 1.57, 76.94),
('elbow_flex_joint', 1.57, 66.18),
('forearm_roll_joint', 1.57, 29.35),
('wrist_flex_joint', 2.26, 25.70),
('wrist_roll_joint', 2.26, 7.36),
]
arm_efforts = [min(1.0, max(-1.0, action[self.joint_name_map.get(name)])) * torque_scale * 0.75 for name, vel_scale, torque_scale in arm_joints]
arm_joint_names = [name for name, vel_scale, torque_scale in arm_joints]
if 1:
arm_joint_names.append('gravity_compensation')
arm_efforts.append(1.0)
arm_msg = JointTrajectory(joint_names=arm_joint_names, points=[JointTrajectoryPoint(effort = arm_efforts)])
self.arm_effort_pub.publish(arm_msg)
with self.timeseq_mutex:
if self.timeseq:
state = dict([(jn, {
'__type': 'WeightlessTorqueAction',
'action': action[self.joint_name_map.get(jn)],
'effort': arm_efforts[i],
}) for i, jn in enumerate(arm_joint_names)])
state['__type'] = 'FetchArmWeightlessTorqueAction'
self.timeseq.add(time.time(), 'arm_action', state)
def set_gripper_action(self, action):
grip = action[self.joint_name_map.get('l_gripper_finger_joint')]
goal = GripperCommandGoal()
if grip > 0:
goal.command.max_effort = 60*min(1.0, grip)
goal.command.position = 0.0
else:
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
with self.timeseq_mutex:
if self.timeseq:
state = {
'__type': 'FetchGripperAction',
'action': grip,
'effort': goal.command.max_effort,
'pos': goal.command.position,
}
self.timeseq.add(time.time(), 'gripper_action', state)
def set_waldo_action(self, joy):
twist = Twist()
twist.linear.x = joy.axes[0]
twist.linear.y = joy.axes[1]
twist.linear.z = joy.axes[2]
twist.angular.x = +3.0*joy.axes[3]
twist.angular.y = +3.0*joy.axes[4]
twist.angular.z = +2.0*joy.axes[5]
self.arm_cartesian_twist_pub.publish(twist)
if joy.buttons[1] and not self.prev_joy_buttons[1]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.0
self.gripper_client.send_goal(goal)
elif not joy.buttons[1] and self.prev_joy_buttons[1]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
def spacenav_joy_cb(self, joy):
if 0: logger.info('joy %s', str(joy))
if joy.buttons[0] and not self.prev_joy_buttons[0]:
if self.waldo_mode:
self.stop_waldo_mode()
elif not self.moving_mode:
self.start_waldo_mode()
if self.waldo_mode and not self.moving_mode:
self.set_waldo_action(joy)
self.prev_joy_buttons = joy.buttons
def start_waldo_mode(self):
logger.info('Start waldo mode');
self.waldo_mode = True
self.start_timeseq('fetchwaldo')
self.start_axis_video()
def stop_waldo_mode(self):
logger.info('Stop waldo mode');
self.waldo_mode = False
timeseq_url = None
if self.timeseq:
timeseq_url = self.timeseq.get_url()
logger.info('save_logs url=%s', timeseq_url)
self.stop_axis_video()
self.close_timeseq()
mts_thread = threading.Thread(target=self.move_to_start)
mts_thread.daemon = True
mts_thread.start()
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
rospy.loginfo("get new grasps")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TODO: ADD BOX
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
# added + .1
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
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", 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 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 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 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 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.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
# here i am declaring goal as an object. https://github.com/mikeferguson/grasping_msgs/blob/master/action/FindGraspableObjects.action
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
# passing the object to find_client
# now find_client is wer magic happens
# on demo.launch file i am runnning basic_grasping_perception. (below <!-- Start Perception -->)
# this keeps running on background and i use actionlib (initalize on init) to get a hook to it.
# find_client is connected to basic_grasping_perception
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
# here we get all the objects
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()
rospy.loginfo("updating scene")
idx = 0
# insert objects to the planning scene
#TODO so these two for loops yo can hardcode the values. try printing all the params and u will understand
for obj in find_result.objects:
rospy.loginfo("object number -> %d" %idx)
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
idx += 1
# for grp in obj.grasps:
# grp.grasp_pose.pose.position.z = 0.37
# this is just minor adjustments i did mess up with this code. just follwed simple gasp thingy
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
rospy.loginfo("height before => %f" % height)
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
rospy.loginfo("height after => %f" % obj.primitive_poses[0].position.z)
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
rospy.loginfo("no. of grasps detected %d dim => %f" % (len(obj.grasps), obj.object.primitives[0].dimensions[0]))
if len(obj.grasps) < 1:
continue
# check size our object is a 0.05^3 cube
if obj.object.primitives[0].dimensions[0] < 0.04 or \
obj.object.primitives[0].dimensions[0] > 0.06 or \
obj.object.primitives[0].dimensions[1] < 0.04 or \
obj.object.primitives[0].dimensions[1] > 0.06 or \
obj.object.primitives[0].dimensions[2] < 0.04 or \
obj.object.primitives[0].dimensions[2] > 0.06:
continue
rospy.loginfo("###### size: x => %f, y => %f, z => %f" % (obj.object.primitive_poses[0].position.x,obj.object.primitive_poses[0].position.y,obj.object.primitive_poses[0].position.z))
# has to be on table
#if obj.object.primitive_poses[0].position.z < 0.5:grasping_client
# continue
return obj.object, obj.grasps
# nothing detected
rospy.loginfo("nothing detected")
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
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
def place(self, block, pose_stamped):
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.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
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.place_with_retry(block.name,
places,
scene=self.scene)
return success
def tuck(self):
# so for each joint i will pass a specific value. first arms are move to pose1 then pose2 then pose3 sequentially
joints = ["shoulder_roll_joint", "shoulder_pitch_joint", "shoulder_yaw_joint",
"elbow_pitch_joint", "elbow_yaw_joint", "wrist_pitch_joint", "wrist_roll_joint"]
pose1 = [1.57079633, 0, 0, 0, 0, 0, 0.0]
pose2 = [1.57079633, 0, -1.57079633, 0, -1.57079633, 0, 0.0]
pose3 = [1.57079633, 1.57079633, -1.57079633, 0, -1.57079633, 1.57079633, 0.0]
while not rospy.is_shutdown():
self.move_group.moveToJointPosition(joints, pose1, 0.02)
self.move_group.moveToJointPosition(joints, pose2, 0.02)
result = self.move_group.moveToJointPosition(joints, pose3, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
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],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
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
def place(self, block, pose_stamped):
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.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
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.place_with_retry(block.name,
places,
scene=self.scene)
return success
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
# Lists of joint angles in the same order as in joint_names
disco_poses = [[0.0, 1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]]
disco_poses1 = [[0.0, 1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0],
[0.133, 0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0],
[0.266, -0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0],
[0.385, -1.5, 1.1, -3.0, -0.5, -3.0, -1.0, -3.0],
[0.266, -0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0],
[0.133, 0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0],
[0.0, 1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]]
for pose in disco_poses:
if rospy.is_shutdown():
break
# Plans the joints in joint_names to angles in pose
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Disco!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
class GraspingClient(object):
def __init__(self, sim=False):
# Define planning groups
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
planner_id = "RRTConnectkConfigDefault"
self.move_group.setPlannerId(planner_id)
self.rmove_group.setPlannerId(planner_id)
self.objects_heights = [0.122, 0.240]
self.objects_heights_tolerances = [0.02, 0.03]
# Define joints and positions for 6 and 7 DOF
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
#self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]
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
]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
# Define the MoveIt pickplace pipeline
self.pickplace = None
self.pickplace = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace.planner_id = planner_id
self.pick_result = None
self.place_result = None
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
find_grasp_planning_topic = "grasp_planner/plan"
rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
self.find_grasp_planning_client = actionlib.SimpleActionClient(
find_grasp_planning_topic, GraspPlanningAction)
self.find_grasp_planning_client.wait_for_server()
rospy.loginfo("...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 __del__(self):
self.scene.clear()
def add_objects_to_keep(self, obj):
self._objs_to_keep.append(obj)
def clearScene(self):
self.scene.clear()
def calculateGraspForObject(self, object_to_grasp, gripper):
goal = GraspPlanningGoal()
goal.object = object_to_grasp
goal.gripper = gripper
self.find_grasp_planning_client.send_goal(goal)
self.find_grasp_planning_client.wait_for_result(rospy.Duration(5.0))
return self.find_grasp_planning_client.get_result(
).grasps #moveit_msgs/Grasp[]
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
retries=1,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
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.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
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.place_with_retry(
block.name, places, retries=1, scene=self.scene)
self.place_result = place_result
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)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.constrained_stow, 0.05)
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,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def open_gripper(self):
self._lgripper.command(self._gripper_open, block=True)
def close_gripper(self):
self._lgripper.command(self._gripper_closed, block=True)
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)
