class pick_place():
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.ur5 = MoveGroupCommander("manipulator")
self.gripper = MoveGroupCommander("end_effector")
print("======================================================")
print(self.robot.get_group_names())
print(self.robot.get_link_names())
print(self.robot.get_joint_names())
print(self.robot.get_planning_frame())
print(self.ur5.get_end_effector_link())
print("======================================================")
self.ur5.set_max_velocity_scaling_factor(0.2)
self.ur5.set_max_acceleration_scaling_factor(0.2)
self.ur5.set_end_effector_link("fts_toolside")
self.ur5.set_planning_time(10.0)
#self.ur5.set_planner_id("RRTkConfigDefault")
self.gripper_ac = RobotiqActionClient('icl_phri_gripper/gripper_controller')
self.gripper_ac.wait_for_server()
self.gripper_ac.initiate()
self.gripper_ac.send_goal(0.08)
self.gripper_ac.wait_for_result()
self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb)
def single_exuete(self, position, mode):
offset = 0.01
rospy.loginfo("let do a single exuete")
rospy.sleep(1)
position_copy = deepcopy(position)
position_copy += [0.14]
position_copy[1] = position_copy[1] + offset
pre_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2])
post_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2])
grasp_position = define_grasp(position_copy)
self.ur5.set_pose_target(pre_position)
rospy.loginfo("let's go to pre position")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
self.ur5.set_pose_target(grasp_position)
rospy.loginfo("let's do this")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
if mode == "pick":
self.gripper_ac.send_goal(0)
if mode == "place":
self.gripper_ac.send_goal(0.08)
self.gripper_ac.wait_for_result()
rospy.loginfo("I got this")
rospy.sleep(1)
self.ur5.set_pose_target(post_position)
rospy.loginfo("move out")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
def pair_exuete(self, pick_position, place_position):
rospy.loginfo("here we go pair")
if pick_position and place_position:
self.single_exuete(pick_position, "pick")
self.single_exuete(place_position, "place")
#rospy.sleep(1)
rospy.loginfo("let's go and get some rest")
rest_position = define_grasp([0.486, -0.152, 0.342])
self.ur5.set_pose_target(rest_position)
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
def pickplace_cb(self, msg):
#print(msg)
print(msg.data)
a = list(msg.data)
mean_x = np.mean([a[i] for i in range(0, len(a)-2, 2)])
mean_y = np.mean([a[i] for i in range(1, len(a)-2, 2)])
num_goals = (len(msg.data) -2)/2
rospy.loginfo("there is {} goals".format(num_goals))
for i in range(0, len(a)-2, 2):
pick_x, pick_y = coord_converter(msg.data[i], msg.data[i+1])
leeway_x = int(msg.data[i] - mean_x)
leeway_y = int(msg.data[i+1] - mean_y)
place_x, place_y = coord_converter(msg.data[-2] + leeway_x, msg.data[-1] + leeway_y)
print(pick_x, pick_y)
print(place_x, place_y)
self.pair_exuete([pick_x, pick_y], [place_x, place_y])
class MoveCup():
def __init__(self):
#basic initiatioon
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_tutorial')
self.robot = moveit_commander.RobotCommander()
################ Collision Object
self.scene = moveit_commander.PlanningSceneInterface()
table = CollisionObject()
primitive = SolidPrimitive()
primitive.type = primitive.BOX
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.stamp = rospy.Time.now()
box_pose.header.frame_id = 'tablelol'
box_pose.pose.position.x = 1.25
box_pose.pose.position.y = 0.0
box_pose.pose.position.z = -0.6
table.primitives.append(primitive)
table.primitive_poses.append(box_pose)
table.operation = table.ADD
self.scene.add_box('table', box_pose, size=(.077, .073, .122))
#use joint_group parameter to change which arm it uses?
self.joint_group = rospy.get_param('~arm', default="left_arm")
self.group = MoveGroupCommander(self.joint_group)
#self.group.set_planner_id("BKPIECEkConfigDefault")
#this node will scale any tf pose requests to be at most max_reach from the base frame
self.max_reach = rospy.get_param('~max_reach', default=1.1)
#define a start pose that we can move to before stuff runs
self.start_pose = PoseStamped()
self.start_pose = self.get_start_pose()
#remove this when working for realz
self.display_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
self.rate = rospy.Rate(1)
self.ik_srv = rospy.ServiceProxy(
'ExternalTools/left/PositionKinematicsNode/IKService',
SolvePositionIK)
self.limb = baxter_interface.Limb('left')
self.rangesub = rospy.Subscriber('cup_center', geometry_msgs.msg.Point,
self.rangecb)
self.stop = False
self.planning = False
def rangecb(self, point):
if self.planning and point.z == 0:
rospy.loginfo('stop')
self.stop = True
self.move_start()
self.rangesub.unregister()
def callback(self, targetarray):
#callback that moves in a constrained path to anything published to /target_poses
##First, scale the position to be withing self.max_reach
#new_target = self.project_point(targetarray.data)
# rospy.loginfo(self.stop)
if not self.stop:
self.planning = True
new_target = self.project_point(targetarray)
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = 'base'
target.pose.position = new_target
#change orientation to be upright
target.pose.orientation = self.start_pose.pose.orientation
#clear group info and set it again
self.group.clear_pose_targets()
# self.group.set_path_constraints(self.get_constraint())
self.group.set_planning_time(10)
# self.group.set_pose_target(target)
################### Try joint space planning
jt_state = JointState()
jt_state.header.stamp = rospy.Time.now()
angles = self.limb.joint_angles()
jt_state.name = list(angles.keys())
jt_state.position = list(angles.values())
jt_state.header.frame_id = 'base'
result = self.ik_srv([target], [jt_state], 0)
angles = {}
i = 0
for name in result.joints[0].name:
angles[name] = result.joints[0].position[i]
i = i + 1
self.group.set_joint_value_target(angles)
#plan and execute plan. If I find a way, I should add error checking her
#currently, if the plan fails, it just doesn't move and waits for another pose to be published
plan = self.group.plan()
self.group.execute(plan)
self.rate.sleep()
return
def scale_movegroup(self, vel=.9, acc=.9):
#slows down baxters arm so we stop getting all those velocity limit errors
self.group.set_max_velocity_scaling_factor(vel)
self.group.set_max_acceleration_scaling_factor(acc)
def unscale_movegroup(self):
self.group.set_max_velocity_scaling_factor(1)
self.group.set_max_acceleration_scaling_factor(1)
def start_baxter_interface(self):
#I copied this from an example but have no idea what it does
self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._left_arm = baxter_interface.limb.Limb("left")
self._left_joint_names = self._left_arm.joint_names()
print(self._left_arm.endpoint_pose())
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# set joint state publishing to 100Hz
self._pub_rate.publish(100)
return
def get_start_pose(self, point=[.9, 0.2, 0], rpy=[0, math.pi / 2, 0]):
#define a starting position for the move_start method
new_p = PoseStamped()
new_p.header.frame_id = self.robot.get_planning_frame()
new_p.pose.position.x = point[0]
new_p.pose.position.y = point[1]
new_p.pose.position.z = point[2]
p_orient = tf.transformations.quaternion_from_euler(
rpy[0], rpy[1], rpy[2])
p_orient = Quaternion(*p_orient)
new_p.pose.orientation = p_orient
return (new_p)
def project_point(self, multiarray):
#scales an array and returns a point (see: Pose.position) to be within self.max_reach
#convert points from sonar ring frame to shoulder frame
x = multiarray.data[2] + sr[0] - lls[0]
y = multiarray.data[0] + sr[1] - lls[1]
z = (-1 * multiarray.data[1]) + sr[2] - lls[2]
#scale point to a finite reach distance from the shoulder
obj_dist = math.sqrt(x**2 + y**2 + z**2)
scale_val = min(self.max_reach / obj_dist, .99)
point_scaled = Point()
#scale point and bring into the base frames
point_scaled.x = scale_val * x + lls[0]
point_scaled.y = scale_val * y + lls[1]
point_scaled.z = scale_val * z + lls[2]
return (point_scaled)
def move_random(self):
#moves baxter to a random position. used for testing
randstate = PoseStamped()
randstate = self.group.get_random_pose()
self.group.clear_pose_targets()
self.group.set_pose_target(randstate)
self.group.set_planning_time(8)
self.scale_movegroup()
plan = self.group.plan()
while len(
plan.joint_trajectory.points) == 1 and not rospy.is_shutdown():
print('plan no work')
plan = self.group.plan()
self.group.execute(plan)
self.rate.sleep()
return
def move_random_constrained(self):
#move baxter to a random position with constrained path planning. also for testing
self.scale_movegroup()
randstate = PoseStamped()
randstate = self.group.get_random_pose()
self.group.clear_pose_targets()
self.group.set_pose_target(randstate)
self.group.set_path_constraints(self.get_constraint())
self.group.set_planning_time(100)
self.scale_movegroup()
constrained_plan = self.group.plan()
self.group.execute(constrained_plan)
self.unscale_movegroup()
rospy.sleep(3)
return
def move_start(self):
#move baxter to the self.start_pose pose
self.group.clear_pose_targets()
self.group.set_pose_target(self.start_pose)
self.group.set_planning_time(10)
print('moving to start')
plan = self.group.plan()
self.group.execute(plan)
print('at start')
self.rate.sleep()
return
def get_constraint(self,
euler_orientation=[0, math.pi / 2, 0],
tol=[3, 3, .5]):
#method takes euler-angle inputs, this converts it to a quaternion
q_orientation = tf.transformations.quaternion_from_euler(
euler_orientation[0], euler_orientation[1], euler_orientation[2])
orientation_msg = Quaternion(q_orientation[0], q_orientation[1],
q_orientation[2], q_orientation[3])
#defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down
constraint = Constraints()
constraint.name = 'upright_wrist'
upright_orientation = OrientationConstraint()
upright_orientation.link_name = self.group.get_end_effector_link()
upright_orientation.orientation = orientation_msg
upright_orientation.absolute_x_axis_tolerance = tol[0]
upright_orientation.absolute_y_axis_tolerance = tol[1]
upright_orientation.absolute_z_axis_tolerance = tol[2]
upright_orientation.weight = 1.0
upright_orientation.header = self.start_pose.header
constraint.orientation_constraints.append(upright_orientation)
return (constraint)
class TestMoveit(unittest.TestCase):
_MOVEGROUP_MAIN = 'manipulator'
_KINEMATICSOLVER_SAFE = 'RRTConnectkConfigDefault'
@classmethod
def setUpClass(self):
rospy.init_node('test_moveit_vs060')
self.robot = RobotCommander()
self._mvgroup = MoveGroupCommander(self._MOVEGROUP_MAIN)
# Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170
self._mvgroup.set_planner_id(self._KINEMATICSOLVER_SAFE)
self._movegroups = sorted(['manipulator', 'manipulator_flange'])
self._joints_movegroup_main = sorted(
['j1', 'j2', 'j3', 'j4', 'j5', 'flange'])
@classmethod
def tearDownClass(self):
True # TODO impl something meaningful
def _set_sample_pose(self):
'''
@return: Pose() with some values populated in.
'''
pose_target = Pose()
pose_target.orientation.x = 0.00
pose_target.orientation.y = 0.00
pose_target.orientation.z = -0.20
pose_target.orientation.w = 0.98
pose_target.position.x = 0.18
pose_target.position.y = 0.18
pose_target.position.z = 0.94
return pose_target
def _plan(self):
'''
Run `clear_pose_targets` at the beginning.
@rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html
'''
self._mvgroup.clear_pose_targets()
pose_target = self._set_sample_pose()
self._mvgroup.set_pose_target(pose_target)
plan = self._mvgroup.plan()
rospy.loginfo(' plan: '.format(plan))
return plan
def test_list_movegroups(self):
'''Check if the defined move groups are loaded.'''
self.assertEqual(self._movegroups,
sorted(self.robot.get_group_names()))
def test_list_activejoints(self):
'''Check if the defined joints in a move group are loaded.'''
self.assertEqual(self._joints_movegroup_main,
sorted(self._mvgroup.get_active_joints()))
def test_plan(self):
'''Evaluate plan (RobotTrajectory)'''
plan = self._plan()
# TODO Better way to check the plan is valid.
# Currently the following checks if the number of waypoints is not zero,
# which (hopefully) indicates that a path is computed.
self.assertNotEqual(0, plan.joint_trajectory.points)
def test_planandexecute(self):
'''
Evaluate Plan and Execute works.
Currently no value checking is done (, which is better to be implemented)
'''
self._plan()
# TODO Better way to check the plan is valid.
# The following checks if plan execution was successful or not.
self.assertTrue(self._mvgroup.go())
def test_set_pose_target(self):
'''
Check for simple planning, originally testd in moved from denso_vs060_moveit_demo_test.py
'''
self._plan()
p = [0.1, -0.35, 0.4]
pose = PoseStamped(
header=rospy.Header(stamp=rospy.Time.now(), frame_id='/BASE'),
pose=Pose(position=Point(*p),
orientation=Quaternion(
*quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
self._mvgroup.set_pose_target(pose)
self.assertTrue(self._mvgroup.go() or self._mvgroup.go())
def test_planning_scene(self):
'''
Check if publish_simple_scene.py is working
'''
rospy.wait_for_service('/get_planning_scene', timeout=20)
get_planning_scene = rospy.ServiceProxy("/get_planning_scene",
GetPlanningScene)
collision_objects = []
# wait for 10 sec
time_start = rospy.Time.now()
while not collision_objects and (rospy.Time.now() -
time_start).to_sec() < 10.0:
world = get_planning_scene(
PlanningSceneComponents(components=PlanningSceneComponents.
WORLD_OBJECT_NAMES)).scene.world
collision_objects = world.collision_objects
rospy.loginfo("collision_objects = " +
str(world.collision_objects))
rospy.sleep(1)
self.assertTrue(world.collision_objects != [], world)
raw_input("please input")
# [00000000]
print right_arm.get_joint_value_target()
print both_arms.get_joint_value_target()
# no this functions
# print right_arm.get_named_targets()
print right_arm.get_remembered_joint_values()
print both_arms.get_remembered_joint_values()
print right_arm.get_path_constraints()
print both_arms.get_path_constraints()
print right_arm.get_active_joints()
print both_arms.get_active_joints()
print right_arm.get_current_joint_values()
print right_arm.get_current_pose()
print right_arm.get_current_rpy()
print both_arms.get_current_joint_values()
print both_arms.get_current_pose()
print both_arms.get_current_rpy()
right_arm.clear_pose_targets()
left_current_pose = both_arms.get_current_pose(end_effector_link='left_gripper').pose
right_current_pose = both_arms.get_current_pose(end_effector_link='right_gripper').pose
print left_current_pose
pose_target.position.x = -0.45 #-0.45
arm_group.set_pose_target(pose_target)
# # # ## Now, we call the planner to compute the plan and execute it.
plan = arm_group.plan(pose_target)
while plan[0] != True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait=True)
arm_group.stop()
arm_group.clear_pose_targets()
# rospy.sleep(1)
#GO FRONT
state = RobotState()
arm_group.set_start_state(state)
pose_target.position.x -= 0.18
arm_group.set_pose_target(pose_target)
plan1 = arm_group.plan(pose_target)
while plan1[0] != True:
state = RobotState()
arm_group.set_start_state(state)
plan1 = arm_group.plan(pose_target)
def main():
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("moveit_test")
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
#gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
#arm.allow_replanning(True)
# Allow 5 seconds per planning attempt
#arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
#$max_pick_attempts = 5
# Give the scene a chance to catch up
#rospy.sleep(0.05)
arm.set_named_target('l_arm_init')
arm.go()
rospy.sleep(0.05)
print("***** arm moved to initial pose")
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 6.98
target_pose.pose.position.y = 2.85
target_pose.pose.position.z = 0.91 + 0.01
q = quaternion_from_euler(0, 0, 0)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
print(target_pose.pose.position.x)
print(target_pose.pose.position.y)
print(target_pose.pose.position.z)
arm.set_pose_target(target_pose)
arm.go()
rospy.sleep(1)
arm.clear_pose_targets()
ps = []
temp = rps_position()
temp.x = 8.0
temp.y = 2.477
temp.z = 0.0
temp.th = 1.944
temp.roll = 0.0
temp.pitch = 0.0
temp.yaw = 0.0
ps.append(temp)
temp2 = rps_position()
temp2.x = 8.3
temp2.y = 3.2
temp2.z = 0.0
temp2.th = 1.5
temp2.roll = 0.0
temp2.pitch = 0.0
temp2.yaw = 0.0
ps.append(temp2)
print ps
callUnitySrvTest(ps)
srv = sp5_control_unity()
srv.unit = 0
srv.cmd = 8
srv.arg = [0]
#callUnityControler(srv)
#r_srv = robot_control()
r_srv = smartpal_control()
r_srv.unit = 0
r_srv.cmd = 8
r_srv.arg = [0] * 8
def main(args):
rospy.init_node('path_data_gen')
# sometimes takes a few tries to connect to robot arm
limb_init = False
while not limb_init:
try:
limb = baxter_interface.Limb('right')
limb_init = True
except OSError:
limb_init = False
neutral_start = limb.joint_angles()
min_goal_cost_threshold = 2.0
# set speed to make sure execution does not violate the speed constraint
limb.set_joint_position_speed(0.65)
# Set up planning scene and Move Group objects
robot = RobotCommander()
group = MoveGroupCommander("right_arm")
sv = StateValidity()
# Setup tables (geometry and location defined in moveit_functions.py, set_environment function)
# set_environment(robot, scene)
# Additional Move group setup
# group.set_goal_joint_tolerance(0.001)
# group.set_max_velocity_scaling_factor(0.7)
# group.set_max_acceleration_scaling_factor(0.1)
max_time = args.max_time
group.set_planning_time(max_time)
# Dictionary to save path data, and filename to save the data to in the end
pathsDict = {}
if not os.path.exists(args.path_data_path):
os.makedirs(args.path_data_path)
pathsFile = args.path_data_path + args.path_data_file
# load data from environment files for obstacle locations and collision free goal poses
# with open("env/environment_data/trainEnvironments_GazeboPatch.pkl", "rb") as env_f:
with open(args.env_data_path + args.env_data_file, "rb") as env_f:
envDict = pickle.load(env_f)
# with open("env/environment_data/trainEnvironments_testGoals.pkl", "rb") as goal_f:
#with open(args.targets_data_path+args.targets_data_file, "rb") as goal_f:
# goalDict = pickle.load(goal_f)
# Obstacle data stored in environment dictionary, loaded into scene modifier to apply obstacle scenes to MoveIt
robot_state = robot.get_current_state()
rs_man = RobotState()
rs_man.joint_state.name = robot_state.joint_state.name
filler_robot_state = list(robot_state.joint_state.position)
goal_sampler = GoalSampler(group, limb, robot_state, sv)
# Here we go
# slice this if you want only some environments
test_envs = envDict['poses'].keys()
done = False
iter_num = 0
print("Testing envs: ")
print(test_envs)
joint_state_topic = ['joint_states:=/robot/joint_states']
roscpp_initialize(joint_state_topic)
rospy.init_node("path_data_gen")
#roscpp_initialize(sys.argv)
# parameters for loading point cloud
executable = './pcd_to_pointcloud'
rootPath = '../data/pcd/'
rospy.wait_for_service('clear_octomap')
clear_octomap = rospy.ServiceProxy('clear_octomap', Empty)
respond = clear_octomap()
print(group.get_end_effector_link())
# try moving the robot to current joints
#group.go(joints=group.get_current_joint_values())
while (not rospy.is_shutdown() and not done):
for i_env, env_name in enumerate(test_envs):
print("env iteration number: " + str(i_env))
print("env name: " + str(env_name))
# load point cloud saved
name = ''
for file in sorted(
os.listdir(args.pcd_data_path),
key=lambda x: int(x.split('Env_')[1].split('_')[1][:-4])):
if (fnmatch.fnmatch(file, env_name + "*")):
# if found the first one that matches the name env+something, then record it
name = file
break
call = create_call(executable, rootPath, name)
### Printing the executable call and allowing user to manually cycle through environments for demonstration
print(call)
### Uncomment below to call pointcloud_to_pcd executable which takes snapshot of the streaming pointcloud data
### and saves it to a .pcd file in a desired file location (as specified by prefix in the command call)
print("Calling executable... \n\n\n")
t = time.time()
#stderr_f = open('log.txt', 'w')
# publishing topic of point cloud for planning
# wait for some time to make sure the point cloud is loaded corretly
p = subprocess.Popen(call) #, stderr=stderr_f)
rospy.sleep(10)
new_pose = envDict['poses'][env_name]
pathsDict[env_name] = {}
pathsDict[env_name]['paths'] = []
pathsDict[env_name]['costs'] = []
pathsDict[env_name]['times'] = []
pathsDict[env_name]['total'] = 0
pathsDict[env_name]['feasible'] = 0
#collision_free_goals = goalDict[env_name]['Joints']
total_paths = 0
feasible_paths = 0
i_path = 0
#group.go(joints=group.get_current_joint_values())
# run until either desired number of total or feasible paths has been found
while (total_paths < args.paths_to_save):
#do planning and save data
# some invalid goal states found their way into the goal dataset, check to ensure goals are "reaching" poses above the table
# by only taking goals which have a straight path cost above a threshold
valid_goal = False
print("FP: " + str(feasible_paths))
print("TP: " + str(total_paths))
total_paths += 1
i_path += 1
# Uncomment below if using a start state different than the robot current state
# filler_robot_state = list(robot_state.joint_state.position) #not sure if I need this stuff
# filler_robot_state[10:17] = moveit_scrambler(start.values())
# rs_man.joint_state.position = tuple(filler_robot_state)
# group.set_start_state(rs_man) # set start
pos = []
while not valid_goal:
pose, joint = goal_sampler.sample()
goal = joint
optimal_path = [neutral_start.values(), goal.values()]
optimal_cost = compute_cost(optimal_path)
if optimal_cost > min_goal_cost_threshold:
valid_goal = True
group.set_start_state_to_current_state()
group.clear_pose_targets()
try:
group.set_joint_value_target(
moveit_scrambler(goal.values())) # set target
except MoveItCommanderException as e:
print(e)
continue
start_t = time.time()
plan = group.plan()
t = time.time() - start_t
#group.execute(plan)
pos = [
plan.joint_trajectory.points[i].positions
for i in range(len(plan.joint_trajectory.points))
]
if pos != []:
pos = np.asarray(pos)
cost = compute_cost(pos)
print("Time: " + str(t))
print("Cost: " + str(cost))
# Uncomment below if using max time as criteria for failure
if (t > (max_time * 0.99)):
print("Reached max time...")
continue
feasible_paths += 1
pathsDict[env_name]['paths'].append(pos)
pathsDict[env_name]['costs'].append(cost)
pathsDict[env_name]['times'].append(t)
pathsDict[env_name]['feasible'] = feasible_paths
pathsDict[env_name]['total'] = total_paths
# Uncomment below if you want to overwrite data on each new feasible path
with open(pathsFile + "_" + env_name + ".pkl",
"wb") as path_f:
pickle.dump(pathsDict[env_name], path_f)
print("\n")
p.terminate()
p.wait()
# rosservice call to clear octomap
rospy.wait_for_service('clear_octomap')
clear_octomap = rospy.ServiceProxy('clear_octomap', Empty)
respond = clear_octomap()
iter_num += 1
print("Env: " + str(env_name))
print("Feasible Paths: " + str(feasible_paths))
print("Total Paths: " + str(total_paths))
print("\n")
pathsDict[env_name]['total'] = total_paths
pathsDict[env_name]['feasible'] = feasible_paths
with open(pathsFile + "_" + env_name + ".pkl", "wb") as path_f:
pickle.dump(pathsDict[env_name], path_f)
print("Done iterating, saving all data and exiting...\n\n\n")
with open(pathsFile + ".pkl", "wb") as path_f:
pickle.dump(pathsDict, path_f)
done = True
class PlannerAnnotationParser(AnnotationParserBase):
"""
Parses the annotations files that contains the benchmarking
information.
"""
def __init__(self, path_to_annotation, path_to_data):
super(PlannerAnnotationParser, self).__init__(path_to_annotation,
path_to_data)
self.parse()
self._load_scene()
self._init_planning()
self.benchmark()
def check_results(self, results):
"""
Returns the results from the planner, checking them against any eventual validation data
(no validation data in our case).
"""
return self.planner_data
def _load_scene(self):
"""
Loads the proper scene for the planner.
It can be either a python static scene or bag containing an occupancy map.
"""
scene = self._annotations["scene"]
for element in scene:
if element["type"] == "launch":
self.play_launch(element["name"])
elif element["type"] == "python":
self.load_python(element["name"])
elif element["type"] == "bag":
self.play_bag(element["name"])
for _ in range(150):
rospy.sleep(0.3)
# wait for the scene to be spawned properly
rospy.sleep(0.5)
def _init_planning(self):
"""
Initialises the needed connections for the planning.
"""
self.group_id = self._annotations["group_id"]
self.planners = self._annotations["planners"]
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(self.group_id)
self._marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10,
latch=True)
self._planning_time_sub = rospy.Subscriber(
'/move_group/result', MoveGroupActionResult,
self._check_computation_time)
rospy.sleep(1)
self.group.set_num_planning_attempts(
self._annotations["planning_attempts"])
self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
self.group.set_planning_time(self._annotations["planning_time"])
self.group.allow_replanning(self._annotations["allow_replanning"])
self._comp_time = []
self.planner_data = []
def benchmark(self):
for test_id, test in enumerate(self._annotations["tests"]):
marker_position_1 = test["start_xyz"]
marker_position_2 = test["goal_xyz"]
self._add_markers(marker_position_1, "Start test \n sequence",
marker_position_2, "Goal")
# Start planning in a given joint position
joints = test["start_joints"]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:6] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
joints = test["goal_joints"]
for planner in self.planners:
if planner == "stomp":
planner = "STOMP"
elif planner == "sbpl":
planner = "AnytimeD*"
self.planner_id = planner
self.group.set_planner_id(planner)
self._plan_joints(
joints,
self._annotations["name"] + "-test_" + str(test_id))
return self.planner_data
def _add_markers(self, point, text1, point_2, text2):
# add marker for start and goal pose of EE
marker_array = MarkerArray()
marker_1 = Marker()
marker_1.header.frame_id = "world"
marker_1.header.stamp = rospy.Time.now()
marker_1.type = Marker.SPHERE
marker_1.scale.x = 0.04
marker_1.scale.y = 0.04
marker_1.scale.z = 0.04
marker_1.lifetime = rospy.Duration()
marker_2 = deepcopy(marker_1)
marker_1.color.g = 0.5
marker_1.color.a = 1.0
marker_1.id = 0
marker_1.pose.position.x = point[0]
marker_1.pose.position.y = point[1]
marker_1.pose.position.z = point[2]
marker_2.color.r = 0.5
marker_2.color.a = 1.0
marker_2.id = 1
marker_2.pose.position.x = point_2[0]
marker_2.pose.position.y = point_2[1]
marker_2.pose.position.z = point_2[2]
marker_3 = Marker()
marker_3.header.frame_id = "world"
marker_3.header.stamp = rospy.Time.now()
marker_3.type = Marker.TEXT_VIEW_FACING
marker_3.scale.z = 0.10
marker_3.lifetime = rospy.Duration()
marker_4 = deepcopy(marker_3)
marker_3.text = text1
marker_3.id = 2
marker_3.color.g = 0.5
marker_3.color.a = 1.0
marker_3.pose.position.x = point[0]
marker_3.pose.position.y = point[1]
marker_3.pose.position.z = point[2] + 0.15
marker_4.text = text2
marker_4.id = 3
marker_4.color.r = 0.5
marker_4.color.a = 1.0
marker_4.pose.position.x = point_2[0]
marker_4.pose.position.y = point_2[1]
marker_4.pose.position.z = point_2[2] + 0.15
marker_array.markers = [marker_1, marker_2, marker_3, marker_4]
self._marker_pub.publish(marker_array)
rospy.sleep(1)
def _plan_joints(self, joints, test_name):
# plan to joint target and determine success
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
plan_time = "N/A"
total_joint_rotation = "N/A"
comp_time = "N/A"
plan_success = self._check_plan_success(plan)
if plan_success:
plan_time = self._check_plan_time(plan)
total_joint_rotation = self._check_plan_total_rotation(plan)
while not self._comp_time:
rospy.sleep(0.5)
comp_time = self._comp_time.pop(0)
self.planner_data.append([
self.planner_id, test_name,
str(plan_success), plan_time, total_joint_rotation, comp_time
])
@staticmethod
def _check_plan_success(plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
@staticmethod
def _check_plan_time(plan):
# find duration of successful plan
number_of_points = len(plan.joint_trajectory.points)
time = plan.joint_trajectory.points[number_of_points -
1].time_from_start.to_sec()
return time
@staticmethod
def _check_plan_total_rotation(plan):
# find total joint rotation in successful trajectory
angles = [0, 0, 0, 0, 0, 0]
number_of_points = len(plan.joint_trajectory.points)
for i in range(number_of_points - 1):
angles_temp = [
abs(x - y)
for x, y in zip(plan.joint_trajectory.points[i + 1].positions,
plan.joint_trajectory.points[i].positions)
]
angles = [x + y for x, y in zip(angles, angles_temp)]
total_angle_change = sum(angles)
return total_angle_change
def _check_computation_time(self, msg):
# get computation time for successful plan to be found
if msg.status.status == 3:
self._comp_time.append(msg.result.planning_time)
def _check_path_length(self, plan):
# find distance travelled by end effector
# not yet implemented
return
class PathPlanner:
"""
This path planner wraps the planning and actuation functionality provided by MoveIt.
All positions are arrays holding x, y, and z coordinates. Orientations are
arrays holding x, y, z, and w coordinates (in quaternion form).
Attributes:
frame_id (str): The frame all coordinates are relative to.
workspace (list): The bounds of the planning space (a box). Specified
as the minimum x, y, and z coordinates, then the maximum x, y, and
z coordinates.
group_name (str): The MoveIt group name (for example, 'right_arm').
time_limit (float): The maximum number of seconds MoveIt will plan for.
robot: The MoveIt robot commander.
scene: The planning scene.
group: The MoveIt MoveGroup.
scene_publisher: A publisher that updates the planning scene.
"""
PLANNING_SCENE_TOPIC = '/collision_object'
def __init__(self, frame_id, group_name, time_limit=10, workspace=None,
register_shutdown=True):
if workspace is None:
workspace = [-2, -2, -2, 2, 2, 2]
if rospy.get_param('verbose'):
rospy.loginfo('Move group: {}'.format(group_name))
self.frame_id, self.workspace = frame_id, workspace
self.time_limit, self.group_name = time_limit, group_name
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self.scene_publisher = rospy.Publisher(self.PLANNING_SCENE_TOPIC,
CollisionObject, queue_size=10)
self.group = MoveGroupCommander(group_name)
if register_shutdown:
rospy.on_shutdown(self.shutdown)
self.group.set_planning_time(time_limit)
self.group.set_workspace(workspace)
rospy.sleep(0.5) # Sleep to ensure initialization has finished.
if rospy.get_param('verbose'):
rospy.loginfo('Initialized path planner.')
def shutdown(self):
""" Stop the path planner safely. """
self.group.stop()
del self.group
if rospy.get_param('verbose'):
rospy.logwarn('Terminated path planner.')
def move_to_pose(self, position, orientation=None):
"""
Move the end effector to the given pose naively.
Arguments:
position: The x, y, and z coordinates to move to.
orientation: The orientation to take (quaternion, optional).
Raises (rospy.ServiceException): Failed to execute movement.
"""
if orientation is None:
self.group.set_position_target(position)
else:
self.group.set_pose_target(create_pose(self.frame_id, position, orientation))
if rospy.get_param('verbose'):
log_pose('Moving to pose.', position, orientation)
self.group.go(wait=True)
self.group.stop()
self.group.clear_pose_targets()
def move_to_pose_with_planner(self, position, orientation=None, orientation_constraints=None):
"""
Move the end effector to the given pose, taking into account
constraints and planning scene obstacles.
Arguments:
position: The x, y, and z coordinates to move to.
orientation: The orientation to take (quaternion, optional).
orientation_constraints (list): A list of `OrientationConstraint`
objects created with `make_orientation_constraint`.
Returns (bool): Whether or not the movement executed successfully.
"""
if orientation_constraints is None:
orientation_constraints = []
target = create_pose(self.frame_id, position, orientation)
if rospy.get_param('verbose'):
log_pose('Moving to pose with planner.', position, orientation)
for _ in range(3):
try:
plan = self.plan_to_pose(target, orientation_constraints)
if not self.group.execute(plan, True):
raise ValueError('Execution failed.')
except ValueError as exc:
rospy.logwarn('Failed to perform movement: {}. Retrying.'.format(str(exc)))
else:
break
else:
raise ValueError('Failed to move to pose.')
def plan_to_pose(self, target, orientation_constraints):
"""
Plan a movement to a pose from the current state, given constraints.
Arguments:
target: The destination pose.
orientation_constraints (list): The constraints.
Returns (moveit_msgs.msg.RobotTrajectory): The path.
"""
self.group.set_pose_target(target)
self.group.set_start_state_to_current_state()
constraints = Constraints()
constraints.orientation_constraints = orientation_constraints
self.group.set_path_constraints(constraints)
return self.group.plan()
def add_box_obstacle(self, dimensions, name, com_position, com_orientation):
"""
Add a rectangular prism obstacle to the planning scene.
Arguments:
dimensions: An array containing the width, length, and height of
the box (in the box's body frame, corresponding to x, y, and z).
name: A unique name for identifying this obstacle.
com_position: The position of the center-of-mass (COM) of this box,
relative to the global frame `frame_id`.
com_orientation: The orientation of the COM.
"""
pose = create_pose(self.frame_id, com_position, com_orientation)
obj = CollisionObject()
obj.id, obj.operation, obj.header = name, CollisionObject.ADD, pose.header
box = SolidPrimitive()
box.type, box.dimensions = SolidPrimitive.BOX, dimensions
obj.primitives, obj.primitive_poses = [box], [pose.pose]
self.scene_publisher.publish(obj)
if rospy.get_param('verbose'):
rospy.loginfo('Added box object "{}" to planning scene: '
'(x={}, y={}, z={}).'.format(name, *dimensions))
def remove_obstacle(self, name):
""" Remove a named obstacle from the planning scene. """
obj = CollisionObject()
obj.id, obj.operation = name, CollisionObject.REMOVE
self.scene_publisher.publish(obj)
if rospy.get_param('verbose'):
rospy.loginfo('Removed object "{}" from planning scene.'.format(name))
def make_orientation_constraint(self, orientation, link_id, tolerance=0.1, weight=1):
"""
Make an orientation constraint in the context of the robot and world.
Arguments:
orientation: The orientation the link should have.
link_id: The name of the link frame.
Returns (OrientationConstraint): The constraint.
"""
constraint = OrientationConstraint()
constraint.header.frame_id = self.frame_id
constraint.link_name = link_id
const_orien = constraint.orientation
const_orien.x, const_orien.y, const_orien.z, const_orien.w = orientation
constraint.absolute_x_axis_tolerance = tolerance
constraint.absolute_y_axis_tolerance = tolerance
constraint.absolute_z_axis_tolerance = tolerance
constraint.weight = weight
return constraint
class MoveItDemo:
def __init__(self):
global obj_att
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'gazebo_world'
initial_pose.pose = target_pose.pose
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T = np.dot(M1, M2)
pre_grasping = deepcopy(target_pose)
pre_grasping.pose.position.x = T[0,3]
pre_grasping.pose.position.y = T[1,3]
pre_grasping.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
pre_grasping.pose.orientation.x = quat[0]
pre_grasping.pose.orientation.y = quat[1]
pre_grasping.pose.orientation.z = quat[2]
pre_grasping.pose.orientation.w = quat[3]
pre_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(pre_grasping)
#################### GRASPING POSE #########################
M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M3[0,3] = target_pose.pose.position.x
M3[1,3] = target_pose.pose.position.y
M3[2,3] = target_pose.pose.position.z
M4 = transformations.euler_matrix(0, 1.57, 0)
M4[0,3] = 0.0 # offset about x
M4[1,3] = 0.0 # about y
M4[2,3] = 0.18 # about z
T2 = np.dot(M3, M4)
grasping = deepcopy(target_pose)
grasping.pose.position.x = T2[0,3]
grasping.pose.position.y = T2[1,3]
grasping.pose.position.z = T2[2,3]
quat2 = transformations.quaternion_from_matrix(T2)
grasping.pose.orientation.x = quat2[0]
grasping.pose.orientation.y = quat2[1]
grasping.pose.orientation.z = quat2[2]
grasping.pose.orientation.w = quat2[3]
grasping.header.frame_id = 'gazebo_world'
self.plan_exec(grasping)
#Close the gripper
print "========== Waiting for gazebo to catch up =========="
self.close_gripper()
#################### ATTACH OBJECT ######################
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links)
# counter to let the planning scene know when to remove the object
obj_att = 1
#self.scene.remove_world_object(target_id)
#################### POST-GRASP RETREAT #########################
M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w])
M5[0,3] = initial_pose.pose.position.x
M5[1,3] = initial_pose.pose.position.y
M5[2,3] = initial_pose.pose.position.z
M6 = transformations.euler_matrix(0, 1.57, 0)
M6[0,3] = 0.0 # offset about x
M6[1,3] = 0.0 # about y
M6[2,3] = 0.3 # about z
T3 = np.dot(M5, M6)
post_grasping = deepcopy(initial_pose)
post_grasping.pose.position.x = T3[0,3]
post_grasping.pose.position.y = T3[1,3]
post_grasping.pose.position.z = T3[2,3]
quat3 = transformations.quaternion_from_matrix(T3)
post_grasping.pose.orientation.x = quat3[0]
post_grasping.pose.orientation.y = quat3[1]
post_grasping.pose.orientation.z = quat3[2]
post_grasping.pose.orientation.w = quat3[3]
post_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(post_grasping)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.52
place_pose.pose.position.y = -0.48
place_pose.pose.position.z = 0.48
place_pose.pose.orientation.w = 1.0
n_attempts = 0
max_place_attempts = 2
# Generate valid place poses
places = self.make_places(place_pose)
success = False
# Repeat until we succeed or run out of attempts
while success == False and n_attempts < max_place_attempts:
for place in places:
success = self.right_arm.place(target_id, place)
if success:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
self.open_gripper()
obj_att = None
rospy.sleep(3)
## # Initialize the grasp object
## g = Grasp()
## grasps = []
## # Set the first grasp pose to the input pose
## g.grasp_pose = pre_grasping
## g.allowed_touch_objects = [target_id]
## grasps.append(deepcopy(g))
## right_arm.pick(target_id, grasps)
# #Change the frame_id for the planning to take place!
# #target_pose.header.frame_id = 'gazebo_world'
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
##################################################################################################################
#Get pose from Gazebo
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
# Generate a list of possible place poses
def make_places(self, init_pose):
# Initialize the place location as a PoseStamped message
place = PoseStamped()
# Start with the input place pose
place = init_pose
# A list of x shifts (meters) to try
x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of pitch angles to try
#pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
pitch_vals = [0]
# A list of yaw angles to try
yaw_vals = [0]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for y in yaw_vals:
for p in pitch_vals:
for y in y_vals:
for x in x_vals:
place.pose.position.x = init_pose.pose.position.x + x
place.pose.position.y = init_pose.pose.position.y + y
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, p, y)
# Set the place pose orientation accordingly
place.pose.orientation.x = q[0]
place.pose.orientation.y = q[1]
place.pose.orientation.z = q[2]
place.pose.orientation.w = q[3]
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
if obj_att is None:
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
# Publish targe's frame
#self.object_frames_pub.publish(target_pose)
threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
def main():
rospy.init_node(sys.argv[1])
# sometimes takes a few tries to connect to robot arm
limb_init = False
while not limb_init:
try:
limb = baxter_interface.Limb('right')
limb_init = True
except OSError:
limb_init = False
neutral_start = limb.joint_angles()
min_goal_cost_threshold = 2.0
# Set up planning scene and Move Group objects
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher(
'planning_scene', PlanningScene, queue_size=0)
robot = RobotCommander()
group = MoveGroupCommander("right_arm")
sv = StateValidity()
set_environment(robot, scene)
# Setup tables (geometry and location defined in moveit_functions.py, set_environment function)
# set_environment(robot, scene)
# Additional Move group setup
# group.set_goal_joint_tolerance(0.001)
# group.set_max_velocity_scaling_factor(0.7)
# group.set_max_acceleration_scaling_factor(0.1)
max_time = 300
group.set_planning_time(max_time)
# Dictionary to save path data, and filename to save the data to in the end
pathsDict = {}
pathsFile = "data/path_data_example"
# load data from environment files for obstacle locations and collision free goal poses
with open("env/trainEnvironments.pkl", "rb") as env_f:
envDict = pickle.load(env_f)
with open("env/trainEnvironments_testGoals.pkl", "rb") as goal_f:
goalDict = pickle.load(goal_f)
# Obstacle data stored in environment dictionary, loaded into scene modifier to apply obstacle scenes to MoveIt
sceneModifier = PlanningSceneModifier(envDict['obsData'])
sceneModifier.setup_scene(scene, robot, group)
robot_state = robot.get_current_state()
rs_man = RobotState()
rs_man.joint_state.name = robot_state.joint_state.name
# Here we go
test_envs = envDict['poses'].keys() #slice this if you want only some environments
done = False
iter_num = 0
print("Testing envs: ")
print(test_envs)
while(not rospy.is_shutdown() and not done):
for i_env, env_name in enumerate(test_envs):
print("env iteration number: " + str(i_env))
print("env name: " + str(env_name))
sceneModifier.delete_obstacles()
new_pose = envDict['poses'][env_name]
sceneModifier.permute_obstacles(new_pose)
print("Loaded new pose and permuted obstacles\n")
pathsDict[env_name] = {}
pathsDict[env_name]['paths'] = []
pathsDict[env_name]['costs'] = []
pathsDict[env_name]['times'] = []
pathsDict[env_name]['total'] = 0
pathsDict[env_name]['feasible'] = 0
collision_free_goals = goalDict[env_name]['Joints']
total_paths = 0
feasible_paths = 0
i_path = 0
while (total_paths < 30): #run until either desired number of total or feasible paths has been found
#do planning and save data
# some invalid goal states found their way into the goal dataset, check to ensure goals are "reaching" poses above the table
# by only taking goals which have a straight path cost above a threshold
valid_goal = False
while not valid_goal:
goal = collision_free_goals[np.random.randint(0, len(collision_free_goals))]
optimal_path = [neutral_start.values(), goal.values()]
optimal_cost = compute_cost(optimal_path)
if optimal_cost > min_goal_cost_threshold:
valid_goal = True
print("FP: " + str(feasible_paths))
print("TP: " + str(total_paths))
total_paths += 1
i_path += 1
# Uncomment below if using a start state different than the robot current state
# filler_robot_state = list(robot_state.joint_state.position) #not sure if I need this stuff
# filler_robot_state[10:17] = moveit_scrambler(start.values())
# rs_man.joint_state.position = tuple(filler_robot_state)
# group.set_start_state(rs_man) # set start
group.set_start_state_to_current_state()
group.clear_pose_targets()
try:
group.set_joint_value_target(moveit_scrambler(goal.values())) # set target
except MoveItCommanderException as e:
print(e)
continue
start_t = time.time()
plan = group.plan()
pos = [plan.joint_trajectory.points[i].positions for i in range(len(plan.joint_trajectory.points))]
if pos != []:
pos = np.asarray(pos)
cost = compute_cost(pos)
t = time.time() - start_t
print("Time: " + str(t))
print("Cost: " + str(cost))
# Uncomment below if using max time as criteria for failure
if (t > (max_time*0.99)):
print("Reached max time...")
continue
feasible_paths += 1
pathsDict[env_name]['paths'].append(pos)
pathsDict[env_name]['costs'].append(cost)
pathsDict[env_name]['times'].append(t)
pathsDict[env_name]['feasible'] = feasible_paths
pathsDict[env_name]['total'] = total_paths
# Uncomment below if you want to overwrite data on each new feasible path
with open (pathsFile + "_" + env_name + ".pkl", "wb") as path_f:
pickle.dump(pathsDict[env_name], path_f)
print("\n")
sceneModifier.delete_obstacles()
iter_num += 1
print("Env: " + str(env_name))
print("Feasible Paths: " + str(feasible_paths))
print("Total Paths: " + str(total_paths))
print("\n")
pathsDict[env_name]['total'] = total_paths
pathsDict[env_name]['feasible'] = feasible_paths
with open(pathsFile + "_" + env_name + ".pkl", "wb") as path_f:
pickle.dump(pathsDict[env_name], path_f)
print("Done iterating, saving all data and exiting...\n\n\n")
with open(pathsFile + ".pkl", "wb") as path_f:
pickle.dump(pathsDict, path_f)
done = True
class MoveItObstaclesDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
self.arm = MoveGroupCommander('manipulator')
# 设置机械臂工作空间
self.arm.set_workspace([-100, -100, 0, 100, 0.3, 100])
# 设置机械臂最大速度
self.arm.set_max_velocity_scaling_factor(value=0.1)
# 获取终端link的名称
self.end_effector_link = self.arm.get_end_effector_link()
rospy.loginfo('end effector link: {}'.format(self.end_effector_link))
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
# self.arm.allow_replanning(True)
self.arm.set_num_planning_attempts(10)
# self.arm.allow_looking(True)
# 设置目标位置所使用的参考坐标系
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
# 设置每次运动规划的时间限制：5s
self.arm.set_planning_time(3)
# 控制机械臂先回到初始化位置
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(2)
def planning(self, start_point, end_point):
"""
功能：动态避障
:param start_point: 起始点, type: dict
:param end_point: 终点, type: dict
:return: None
"""
# 移动机械臂到指定位置，并获取当前位姿数据为机械臂运动的起始位姿
if start_point:
self.move_arm(p=start_point)
self.start_pose = self.arm.get_current_pose(self.end_effector_link).pose
# 使用moveit自带的求解器规划出A到B的离散路径点, 并存到列表way_points中
# way_points = self.get_way_points(start_point, end_point)
while True:
print('set planner id: RRT')
self.arm.set_planner_id('TRRTkConfigDefault')
self.arm.set_named_target('up')
self.arm.go()
rospy.sleep(5)
print('set planner id: PRM')
self.arm.set_planner_id('TRRTkConfigDefault')
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(5)
# -------------------------------------------------------------------
def get_way_points(self, a, b):
way_points = []
# plan 1
self.arm.set_named_target('up')
traj = self.arm.plan()
if traj.joint_trajectory.points: # True if trajectory contains points
rospy.loginfo("get trajectory success")
else:
rospy.logerr("Trajectory is empty. Planning false!")
self.arm.clear_pose_targets()
# plan 2
# traj = self.moveit_planning(p=b)
for i, p in enumerate(traj.joint_trajectory.points):
# rospy.loginfo('waypoint: {}'.format(i))
if i%2 == 0:
point = {
'x': p.positions[0],
'y': p.positions[1],
'z': p.positions[2],
'ox': p.positions[3],
'oy': p.positions[4],
'oz': p.positions[5]
}
way_points.append(point)
rospy.loginfo('waypoint: \n {}'.format(way_points))
rospy.loginfo(len(way_points))
return way_points
def moveit_planning(self, p):
"""
使用moveit求解器规划路径
:param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
:return:
"""
rospy.loginfo('start moveit planning...')
target_pose = PoseStamped()
target_pose.header.frame_id = self.reference_frame
target_pose.pose.position.x = p['x']
target_pose.pose.position.y = p['y']
target_pose.pose.position.z = p['z']
if 'ox' in p.keys() and p['ox']:
target_pose.pose.orientation.x = p['ox']
if 'oy' in p.keys() and p['oy']:
target_pose.pose.orientation.y = p['oy']
if 'oz' in p.keys() and p['oz']:
target_pose.pose.orientation.z = p['oz']
if 'ow' in p.keys() and p['ow']:
target_pose.pose.orientation.w = p['ow']
# 传入一个PoseStamped
# self.arm.set_pose_target(target_pose, self.end_effector_link)
# 尝试直接传入一个列表
self.arm.set_pose_target([p['x'], p['y'], p['z'], p['ox'], p['oy'], p['oz']], self.end_effector_link)
traj = self.arm.plan()
if traj.joint_trajectory.points: # True if trajectory contains points
rospy.loginfo("get trajectory success")
return traj
else:
rospy.logerr("Trajectory is empty. Planning false!")
def move_arm(self, p):
"""
移动机械臂到p点
:param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
:return:
"""
rospy.loginfo('start arm moving...')
traj = self.moveit_planning(p)
self.arm.execute(traj)
# 设置当前位置为起始位置
self.arm.set_start_state_to_current_state()
rospy.sleep(1)
def stop_arm(self):
"""
急停
:return:
"""
pass
def exist_danger_obstacle(self):
"""
环境中是否存在危险的障碍物
:return: True or False
"""
return False
def get_obstacle_octomap(self):
"""
获取环境的octomap信息
:return:
"""
pass
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(3)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(3)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
"""
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
rospy.sleep(5)
"""
# 设置桌面的高度
table_ground = 0.25
# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.8]
box2_size = [0.05, 0.05, 0.8]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.46
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
# scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.41
box1_pose.pose.position.y = -0.3
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
# scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.39
box2_pose.pose.position.y = 0.3
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
# scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 将场景中的颜色设置发布
# self.sendColors()
# 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
target_pose = Pose()
target_pose.position.x = 0.4
target_pose.position.y = 0.0
target_pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.5
target_pose.orientation.w = 1.0
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose)
arm.go()
rospy.sleep(5)
arm.stop()
arm.clear_pose_targets()
# 设置机械臂的运动目标位置，进行避障规划
target_pose2 = PoseStamped()
target_pose2.header.frame_id = reference_frame
target_pose2.pose.position.x = 0.4
target_pose2.pose.position.y = 0.5
target_pose2.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.4
target_pose2.pose.orientation.w = 1.0
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose2, end_effector_link)
arm.go()
rospy.sleep(5)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class MoveIt(object):
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.add_table()
# self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.arm = MoveGroupCommander("arm")
# self.arm.set_goal_joint_tolerance(0.1)
self.gripper = MoveGroupCommander("gripper")
# already default
self.arm.set_planner_id("RRTConnectkConfigDefault")
self.end_effector_link = self.arm.get_end_effector_link()
self.arm.allow_replanning(True)
self.arm.set_planning_time(5)
self.transformer = tf.TransformListener()
rospy.sleep(2) # allow some time for initialization of moveit
def __del__(self):
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def _open_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [0, 0]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
def _set_gripper_width(self, width):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [0, 0]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
def _close_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [1.2, 1.2]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
# Template function for creating the Grasps
def _create_grasp(self, x, y, z, roll, pitch, yaw):
grasp = Grasp()
# pre_grasp
grasp.pre_grasp_posture = self._open_gripper()
grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
grasp.pre_grasp_approach.direction.vector.z = 1.0
grasp.pre_grasp_approach.direction.vector.y = 0.0
grasp.pre_grasp_approach.direction.vector.x = 0.0
grasp.pre_grasp_approach.min_distance = 0.05
grasp.pre_grasp_approach.desired_distance = 0.1
# grasp
grasp.grasp_posture = self._close_gripper()
grasp.grasp_pose.pose.position.x = x
grasp.grasp_pose.pose.position.y = y
grasp.grasp_pose.pose.position.z = z
q = quaternion_from_euler(roll, pitch, yaw)
grasp.grasp_pose.pose.orientation.x = q[0]
grasp.grasp_pose.pose.orientation.y = q[1]
grasp.grasp_pose.pose.orientation.z = q[2]
grasp.grasp_pose.pose.orientation.w = q[3]
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"
# post_grasp
grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
grasp.post_grasp_retreat.direction.vector.z = -1.0
grasp.post_grasp_retreat.direction.vector.x = 0.0
grasp.post_grasp_retreat.direction.vector.y = 0.0
grasp.post_grasp_retreat.min_distance = 0.05
grasp.post_grasp_retreat.desired_distance = 0.25
return [grasp]
# Template function, you can add parameters if needed!
def grasp(self, x, y, z, roll, pitch, yaw, width):
self.add_object('object', [0.37, -0.24, 0.1, math.pi, 0., 0.],
[0.1, 0.1, 0.1])
grasps = self._create_grasp(x, y, z, roll, pitch, yaw)
result = self.arm.pick('object', grasps)
self.remove_object()
if result == MoveItErrorCodes.SUCCESS:
print 'Success grasp'
return True
else:
print 'Failed grasp'
return False
def open_fingers(self):
self.gripper.set_joint_value_target([0.0, 0.0])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def close_fingers(self):
self.gripper.set_joint_value_target([1.3, 1.3])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def move_to(self,
x,
y,
z,
roll,
pitch,
yaw,
frame_id="m1n6s200_link_base"):
q = quaternion_from_euler(roll, pitch, yaw)
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = True
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def remove_object(self, object='object'):
self.scene.remove_attached_object(self.end_effector_link, object)
self.scene.remove_world_object(object)
rospy.loginfo("Object removed")
def add_object(self, name, pose, size):
object_pose = PoseStamped()
object_pose.header.frame_id = "m1n6s200_link_base"
object_pose.pose.position.x = pose[0]
object_pose.pose.position.y = pose[1]
object_pose.pose.position.z = pose[2]
q = quaternion_from_euler(*pose[3:])
object_pose.pose.orientation.x = q[0]
object_pose.pose.orientation.y = q[1]
object_pose.pose.orientation.z = q[2]
object_pose.pose.orientation.w = q[3]
self.scene.add_box(name, object_pose, size)
def add_table(self):
self.add_object('table', [0, 0, -0.005, 0, 0, 0], (2, 2, 0.01))
def main():
roscpp_initialize(sys.argv)
rospy.init_node('grasp_ur5', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()
arm_group = MoveGroupCommander("manipulator")
q = quaternion_from_euler(1.5701, 0.0, -1.5701)
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = q[0]
pose_target.orientation.y = q[1]
pose_target.orientation.z = q[2]
pose_target.orientation.w = q[3]
pose_target.position.z = 0.23 #0.23
pose_target.position.y = 0.11 #0.11
pose_target.position.x = -0.45 #-0.45
arm_group.set_pose_target(pose_target)
plan = arm_group.plan(pose_target)
while plan[0]!= True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait = True)
arm_group.stop()
arm_group.clear_pose_targets()
# rospy.sleep(1)
#FAKE PLAN WITHOUT RESTRICTIONS #
# state = RobotState()
# arm_group.set_start_state(state)
pose_target.position.z = 0.77
arm_group.set_pose_target(pose_target)
plan_fake = arm_group.plan(pose_target)
while plan_fake[0] != True:
plan_fake = arm_group.plan(pose_target)
pose = arm_group.get_current_pose()
constraint = Constraints()
constraint.name = "restricao"
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = arm_group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 3.14
orientation_constraint.absolute_y_axis_tolerance = 3.14
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1
constraint.orientation_constraints.append(orientation_constraint)
arm_group.set_path_constraints(constraint)
# state = RobotState()
# arm_group.set_start_state(state)
pose_target.position.z = 0.77 # 0.77
# pose_target.position.y = -0.11 # -0.11
# pose_target.position.x = 0.31 # 0.31
arm_group.set_pose_target(pose_target)
plan = arm_group.plan(pose_target)
while plan[0]!= True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait = True)
arm_group.stop()
arm_group.clear_pose_targets()
class MoveIt(object):
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.arm = MoveGroupCommander("arm")
# self.arm.set_goal_joint_tolerance(0.1)
self.gripper = MoveGroupCommander("gripper")
# already default
self.arm.set_planner_id("RRTConnectkConfigDefault")
self.end_effector_link = self.arm.get_end_effector_link()
self.arm.allow_replanning(True)
self.arm.set_planning_time(5)
self.transformer = tf.TransformListener()
rospy.sleep(2) # allow some time for initialization of moveit
def __del__(self):
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def _open_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [0, 0]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
def _close_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [1.2, 1.2]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
# Template function for creating the Grasps
def _create_grasps(self, x, y, z, z_max, rotation):
grasps = []
# You can create multiple grasps and add them to the grasps list
grasp = Grasp() # create a new grasp
# Set the pre grasp posture (the fingers)
grasp.pre_grasp_posture = self._open_gripper()
# Set the grasp posture (the fingers)
grasp.grasp_posture = self._close_gripper()
# Set the position of where to grasp
grasp.grasp_pose.pose.position.x = x
grasp.grasp_pose.pose.position.y = y
grasp.grasp_pose.pose.position.z = z
# Set the orientation of the end effector
q = quaternion_from_euler(math.pi, 0.0, rotation)
grasp.grasp_pose.pose.orientation.x = q[0]
grasp.grasp_pose.pose.orientation.y = q[1]
grasp.grasp_pose.pose.orientation.z = q[2]
grasp.grasp_pose.pose.orientation.w = q[3]
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"
# Set the pre_grasp_approach
grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
grasp.pre_grasp_approach.direction.vector.z = 1.0
grasp.pre_grasp_approach.direction.vector.y = 0.0
grasp.pre_grasp_approach.direction.vector.x = 0.0
grasp.pre_grasp_approach.min_distance = 0.05
grasp.pre_grasp_approach.desired_distance = 0.1
# # Set the post_grasp_approach
grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
grasp.post_grasp_retreat.direction.vector.z = -1.0
grasp.post_grasp_retreat.direction.vector.x = 0.0
grasp.post_grasp_retreat.direction.vector.y = 0.0
grasp.post_grasp_retreat.min_distance = 0.05
grasp.post_grasp_retreat.desired_distance = 0.25
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # setting the planning frame (Positive x is to the left, negative Y is to the front of the arm)
grasps.append(
grasp
) # add all your grasps in the grasps list, MoveIT will pick the best one
for z_offset in np.arange(z + 0.02, z_max, 0.01):
new_grasp = copy.deepcopy(grasp)
new_grasp.grasp_pose.pose.position.z = z_offset
grasps.append(new_grasp)
return grasps
# Template function, you can add parameters if needed!
def grasp(self, x, y, z, z_max, rotation, size):
print '******************* grasp'
# Object distance:
obj_dist = np.linalg.norm(np.asarray((x, y, z)))
if obj_dist > 0.5:
rospy.loginfo(
"Object too far appart ({} m), skipping pick".format(obj_dist))
return False
# Add collision object, easiest to name the object, "object"
object_pose = PoseStamped()
object_pose.header.frame_id = "m1n6s200_link_base"
object_pose.pose.position.x = x
object_pose.pose.position.y = y
object_pose.pose.position.z = z
q = quaternion_from_euler(math.pi, 0.0, rotation)
object_pose.pose.orientation.x = q[0]
object_pose.pose.orientation.y = q[1]
object_pose.pose.orientation.z = q[2]
object_pose.pose.orientation.w = q[3]
self.scene.add_box("object", object_pose, size)
rospy.sleep(0.5)
self.clear_octomap()
rospy.sleep(1.0)
# Create and return grasps
# z += size[2]/2 # Focus on the top of the object only
# z += size[2]/2 + 0.02 # Focus on the top of the object only
grasps = self._create_grasps(x, y, z, z_max, rotation)
print '******************************************************************************'
result = self.arm.pick(
'object', grasps) # Perform pick on "object", returns result
print '******************************************************************************'
# self.move_to(x, y, z + 0.15, rotation)
if result == MoveItErrorCodes.SUCCESS:
print 'Success grasp'
return True
else:
print 'Failed grasp'
return False
def clear_object(self, x, y, z, z_max, rotation, size):
print '******************* clear_object'
self.move_to_waypoint()
success = self.grasp(x, y, z, z_max, rotation, size)
if success:
self.move_to_waypoint()
success = self.move_to_drop_zone()
if success:
print 'success move to drop zone'
else:
print 'failed move to drop zone'
self.open_fingers()
self.remove_object()
rospy.sleep(1.0)
self.close_fingers()
return success
def open_fingers(self):
print '******************* open_fingers'
self.gripper.set_joint_value_target([0.0, 0.0])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def close_fingers(self):
print '******************* close_fingers'
self.gripper.set_joint_value_target([1.3, 1.3])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def move_to(self, x, y, z, rotation, frame_id="m1n6s200_link_base"):
print '******************* move_to'
q = quaternion_from_euler(math.pi, 0.0, rotation)
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def move_to_waypoint(self):
print '******************* move_to_waypoint'
return self.move_to(0.35, 0, 0.25, 1.57)
def rtb(self):
print '******************* rtb'
self.move_to_drop_zone()
# pose = PoseStamped()
# pose.header.frame_id = 'base_footprint'
# pose.pose.position.x = -0.191258927921
# pose.pose.position.y = 0.1849306168113
# pose.pose.position.z = 0.813729734732
# pose.pose.orientation.x = -0.934842026356
# pose.pose.orientation.y = 0.350652799078
# pose.pose.orientation.z = -0.00168532388516
# pose.pose.orientation.w = 0.0557688079539
#
# self.arm.set_pose_target(pose, self.end_effector_link)
# plan = self.arm.plan()
# self.arm.go(wait=True)
# self.arm.stop()
# self.arm.clear_pose_targets()
def move_to_drop_zone(self):
print '******************* move_to_drop_zone'
pose = PoseStamped()
pose.header.frame_id = "m1n6s200_link_base"
pose.pose.position.x = 0.2175546259709541
pose.pose.position.y = 0.18347985269448372
pose.pose.position.z = 0.16757751444136426
pose.pose.orientation.x = 0.6934210704552356
pose.pose.orientation.y = 0.6589390059796749
pose.pose.orientation.z = -0.23223137602833943
pose.pose.orientation.w = -0.17616808290725341
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def print_position(self):
pose = self.arm.get_current_pose()
self.transformer.waitForTransform("m1n6s200_link_base",
"base_footprint", rospy.Time.now(),
rospy.Duration(10))
eef_pose = self.transformer.transformPose("m1n6s200_link_base", pose)
orientation = eef_pose.pose.orientation
orientation = [
orientation.x, orientation.y, orientation.z, orientation.w
]
euler = euler_from_quaternion(orientation)
print "z:", eef_pose.pose.position.x
print "y:", eef_pose.pose.position.y
print "z:", eef_pose.pose.position.z
print "yaw (degrees):", math.degrees(euler[2])
def remove_object(self):
self.scene.remove_attached_object(self.end_effector_link, "object")
self.scene.remove_world_object("object")
rospy.loginfo("Object removed")
class TestPlanners(object):
def __init__(self, group_id, planner):
rospy.init_node('moveit_test_planners', anonymous=True)
self.pass_list = []
self.fail_list = []
self.planner = planner
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(group_id)
rospy.sleep(1)
self.group.set_planner_id(self.planner)
self.test_trajectories_empty_environment()
self.test_waypoints()
self.test_trajectories_with_walls_and_ground()
for pass_test in self.pass_list:
print(pass_test)
for fail_test in self.fail_list:
print(fail_test)
def _add_walls_and_ground(self):
# publish a scene
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below ground (to prevent collision with
# the robot itself)
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.1))
p.pose.position.x = 0.4
p.pose.position.y = 0.85
p.pose.position.z = 0.4
p.pose.orientation.x = 0.5
p.pose.orientation.y = -0.5
p.pose.orientation.z = 0.5
p.pose.orientation.w = 0.5
self.scene.add_box("wall_front", p, (0.8, 2, 0.01))
p.pose.position.x = 1.33
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707388
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.706825
self.scene.add_box("wall_right", p, (0.8, 2, 0.01))
p.pose.position.x = -0.5
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707107
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.707107
self.scene.add_box("wall_left", p, (0.8, 2, 0.01))
# rospy.sleep(1)
def _check_plan(self, plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
def _plan_joints(self, joints):
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
return self._check_plan(plan)
def test_trajectories_rotating_each_joint(self):
# test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2]
test_joint_values = [numpy.pi / 2.0]
joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0]
# Joint 4th is colliding with the hand
# for joint in range(6):
for joint in [0, 1, 2, 3, 5]:
for value in test_joint_values:
joints[joint] = value
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
def test_trajectories_empty_environment(self):
# Up - Does not work with sbpl but it does with ompl
joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# All joints up
joints = [
-1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Down
joints = [
-0.000348431194526, 0.397651011661, 0.0766181197394,
-0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# left
joints = [
0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765,
0.146075718452, 0.00420656698366, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Front
joints = [
1.425279839, -0.110370375874, -1.52548746261, -1.50659865247,
-1.42700242769, 3.1415450794, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Behind
joints = [
1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839,
0.694689321451, -0.390769365032, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Should fail because it is in self-collision
joints = [
-0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181,
1.04235601797, -1.69730925867, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
def test_waypoints(self):
# Start planning in a given joint position
joints = [
-0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979,
0.344227622185, -3.03250264451, 0.0, 0.0
]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:8] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
waypoints = []
initial_pose = self.group.get_current_pose().pose
initial_pose.position.x = -0.301185959729
initial_pose.position.y = 0.517069787724
initial_pose.position.z = 1.20681710541
initial_pose.orientation.x = 0.0207499700474
initial_pose.orientation.y = -0.723943002716
initial_pose.orientation.z = -0.689528413407
initial_pose.orientation.w = 0.00515118111221
# start with a specific position
waypoints.append(initial_pose)
# first move it down
wpose = geometry_msgs.msg.Pose()
wpose.orientation = waypoints[0].orientation
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z - 0.20
waypoints.append(copy.deepcopy(wpose))
# second front
wpose.position.y += 0.20
waypoints.append(copy.deepcopy(wpose))
# third side
wpose.position.x -= 0.20
waypoints.append(copy.deepcopy(wpose))
# fourth return to initial pose
wpose = waypoints[0]
waypoints.append(copy.deepcopy(wpose))
(plan3,
fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0)
if not self._check_plan(plan3) and fraction > 0.8:
self.fail_list.append("Failed: test_waypoints, " + self.planner)
else:
self.pass_list.append("Passed: test_waypoints, " + self.planner)
def test_trajectories_with_walls_and_ground(self):
self._add_walls_and_ground()
# Should fail to plan: Goal is in collision with the wall_front
joints = [
0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275,
0.302143499777, 0.00130280337897, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Should fail to plan: Goal is in collision with the ground
joints = [
3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437,
0.000133883149666, -0.594498939239, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Goal close to right corner
joints = [
0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116,
-1.3516255551, 2.8225061435, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed, test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
self.scene.remove_world_object("ground")
self.scene.remove_world_object("wall_left")
self.scene.remove_world_object("wall_right")
self.scene.remove_world_object("wall_front")
class DaArmServer:
"""The basic, design problem/tui agnostic arm server
"""
gestures = {}
pointing_height = 0.06
grasp_height = 0.05
drop_height = 0.07
cruising_height = 0.1
START_TOLERANCE = 0.05 # this is for moveit to check for change in joint angles before moving
GOAL_TOLERANCE = 0.005
moving = False
paused = False
move_group_state = "IDLE"
last_joint_trajectory_goal = ""
last_joint_trajectory_result = ""
def __init__(self, num_planning_attempts=100, safe_zone=None):
rospy.init_node("daarm_server", anonymous=True)
self.safe_zone = safe_zone # this is a fallback zone to drop a block on fail if nothing is passed: [{x,y},{xT,yT}]
self.init_params()
self.init_scene()
self.init_publishers()
self.init_subscribers()
self.init_action_clients()
self.init_service_clients()
self.init_arm(num_planning_attempts)
def init_arm(self, num_planning_attempts=700):
rospy.set_param(
"/move_group/trajectory_execution/allowed_start_tolerance",
self.START_TOLERANCE)
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE)
self.arm.set_goal_orientation_tolerance(0.02)
self.init_services()
self.init_action_servers()
def init_scene(self):
world_objects = [
"table", "tui", "monitor", "overHead", "wall", "farWall",
"frontWall", "backWall", "blockProtector", "rearCamera"
]
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
for obj in world_objects:
self.scene.remove_world_object(obj)
rospy.sleep(0.5)
self.tuiPose = PoseStamped()
self.tuiPose.header.frame_id = self.robot.get_planning_frame()
self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51)
self.tuiDimension = (0.9906, 0.8382, 0.8836)
self.overHeadPose = PoseStamped()
self.overHeadPose.header.frame_id = self.robot.get_planning_frame()
self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97)
self.overHeadDimension = (0.9906, 0.8382, 0.05)
self.blockProtectorPose = PoseStamped()
self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame(
)
self.blockProtectorPose.pose.position = Point(
0.0056, -0.343, -0.51 + self.cruising_height)
self.blockProtectorDimension = (0.9906, 0.8382, 0.8636)
self.wallPose = PoseStamped()
self.wallPose.header.frame_id = self.robot.get_planning_frame()
self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048)
self.wallDimension = (0.6096, 2, 1.35)
self.farWallPose = PoseStamped()
self.farWallPose.header.frame_id = self.robot.get_planning_frame()
self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048)
self.farWallDimension = (0.6096, 2, 3.35)
self.frontWallPose = PoseStamped()
self.frontWallPose.header.frame_id = self.robot.get_planning_frame()
self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51)
self.frontWallDimension = (1, 0.15, 4)
self.backWallPose = PoseStamped()
self.backWallPose.header.frame_id = self.robot.get_planning_frame()
self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51)
self.backWallDimension = (1, 0.005, 4)
self.rearCameraPose = PoseStamped()
self.rearCameraPose.header.frame_id = self.robot.get_planning_frame()
self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51)
self.rearCameraDimension = (0.5, 0.5, 2)
rospy.sleep(0.5)
self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
self.scene.add_box("wall", self.wallPose, self.wallDimension)
self.scene.add_box("farWall", self.farWallPose, self.farWallDimension)
self.scene.add_box("overHead", self.overHeadPose,
self.overHeadDimension)
self.scene.add_box("backWall", self.backWallPose,
self.backWallDimension)
self.scene.add_box("frontWall", self.frontWallPose,
self.frontWallDimension)
self.scene.add_box("rearCamera", self.rearCameraPose,
self.rearCameraDimension)
def raise_table(self):
#raises the table obstacle to protect blocks on the table during transport
self.scene.remove_world_object("blockProtector")
self.scene.add_box("blockProtector", self.blockProtectorPose,
self.blockProtectorDimension)
def lower_table(self):
#lowers the table to allow grasping into it
self.scene.remove_world_object("blockProtector")
def init_params(self):
try:
self.grasp_height = get_ros_param(
"GRASP_HEIGHT", "Grasp height defaulting to 0.01")
self.drop_height = get_ros_param("DROP_HEIGHT",
"Drop height defaulting to 0.07")
self.cruising_height = get_ros_param(
"CRUISING_HEIGHT", "Cruising height defaulting to 0.1")
self.pointing_height = get_ros_param(
"POINT_HEIGHT", "Pointing height defaulting to 0.06")
except ValueError as e:
rospy.loginfo(e)
def handle_param_update(self, message):
self.init_params()
def init_publishers(self):
self.calibration_publisher = rospy.Publisher("/calibration_results",
CalibrationParams,
queue_size=1)
self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs",
String,
queue_size=1)
rospy.sleep(0.5)
def init_subscribers(self):
self.joint_angle_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/joint_angles', JointAngles,
self.update_joints)
self.joint_trajectory_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/status', GoalStatusArray,
self.update_joint_trajectory_state)
self.joint_trajectory_goal_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal, self.update_joint_trajectory_goal)
self.joint_trajectory_result_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/result',
FollowJointTrajectoryActionResult,
self.update_joint_trajectory_result)
self.finger_position_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/finger_position', FingerPosition,
self.update_finger_position)
self.param_update_subscriber = rospy.Subscriber(
"/param_update", String, self.handle_param_update)
self.moveit_status_subscriber = rospy.Subscriber(
'/move_group/status', GoalStatusArray,
self.update_move_group_status)
self.move_it_feedback_subscriber = rospy.Subscriber(
'/move_group/feedback', MoveGroupActionFeedback,
self.update_move_group_state)
#Topic for getting joint torques
rospy.Subscriber('/j2s7s300_driver/out/joint_torques', JointAngles,
self.monitorJointTorques)
#Topic for getting cartesian force on end effector
rospy.Subscriber('/j2s7s300_driver/out/tool_wrench',
geometry_msgs.msg.WrenchStamped,
self.monitorToolWrench)
def init_action_servers(self):
self.calibration_server = actionlib.SimpleActionServer(
"calibrate_arm", CalibrateAction, self.calibrate, auto_start=False)
self.calibration_server.start()
self.move_block_server = actionlib.SimpleActionServer(
"move_block",
MoveBlockAction,
self.handle_move_block,
auto_start=False)
self.move_block_server.start()
#self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm)
self.move_pose_server = actionlib.SimpleActionServer(
"move_pose",
MovePoseAction,
self.handle_move_pose,
auto_start=False)
self.move_pose_server.start()
def init_services(self):
self.home_arm_service = rospy.Service("/home_arm", ArmCommand,
self.handle_home_arm)
# emergency stop
self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand,
self.handle_stop_arm)
# stop and pause for a bit
self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand,
self.handle_pause_arm)
self.start_arm_service = rospy.Service("/restart_arm", ArmCommand,
self.handle_restart_arm)
def init_action_clients(self):
# Action Client for joint control
joint_action_address = '/j2s7s300_driver/joints_action/joint_angles'
self.joint_action_client = actionlib.SimpleActionClient(
joint_action_address, kinova_msgs.msg.ArmJointAnglesAction)
rospy.loginfo('Waiting for ArmJointAnglesAction server...')
self.joint_action_client.wait_for_server()
rospy.loginfo('ArmJointAnglesAction Server Connected')
# Service to move the gripper fingers
finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions'
self.finger_action_client = actionlib.SimpleActionClient(
finger_action_address, kinova_msgs.msg.SetFingersPositionAction)
self.finger_action_client.wait_for_server()
#
def init_service_clients(self):
self.is_simulation = False
try:
self.is_simulation = get_ros_param("IS_SIMULATION", "")
except:
self.is_simulation = False
if self.is_simulation is True:
# setup alternatives to jaco services for emergency stop, joint control, and finger control
pass
# Service to get TUI State
rospy.wait_for_service('get_tui_blocks')
self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState)
# Service for homing the arm
home_arm_service = '/j2s7s300_driver/in/home_arm'
self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm)
rospy.loginfo('Waiting for kinova home arm service')
rospy.wait_for_service(home_arm_service)
rospy.loginfo('Kinova home arm service server connected')
# Service for emergency stop
emergency_service = '/j2s7s300_driver/in/stop'
self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop)
rospy.loginfo('Waiting for Stop service')
rospy.wait_for_service(emergency_service)
rospy.loginfo('Stop service server connected')
# Service for restarting the arm
start_service = '/j2s7s300_driver/in/start'
self.restart_arm = rospy.ServiceProxy(start_service, Start)
rospy.loginfo('Waiting for Start service')
rospy.wait_for_service(start_service)
rospy.loginfo('Start service server connected')
def handle_start_arm(self, message):
return self.restart_arm()
def handle_stop_arm(self, message):
return self.stop_motion()
def handle_pause_arm(self, message):
self.stop_motion(home=True, pause=True)
return str(self.paused)
def handle_restart_arm(self, message):
self.restart_arm()
self.paused = False
return str(self.paused)
def handle_home_arm(self, message):
try:
status = self.home_arm()
return json.dumps(status)
except rospy.ServiceException as e:
rospy.loginfo("Homing arm failed")
def home_arm(self):
# send the arm home
# for now, let's just use the kinova home
#self.home_arm_client()
self.home_arm_kinova()
return "done"
def custom_home_arm(self):
angles_set = [
629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
0.172727271914, 212.423721313, 539.743164062
]
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angles_set[0]
goal.angles.joint2 = angles_set[1]
goal.angles.joint3 = angles_set[2]
goal.angles.joint4 = angles_set[3]
goal.angles.joint5 = angles_set[4]
goal.angles.joint6 = angles_set[5]
goal.angles.joint7 = angles_set[6]
self.joint_action_client.send_goal(goal)
def home_arm_kinova(self):
"""Takes the arm to the kinova default home if possible
"""
# self.arm.set_named_target("Home")
angles_set = map(np.deg2rad, [
629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
0.172727271914, 212.423721313, 269.743164062
])
self.arm.clear_pose_targets()
try:
self.arm.set_joint_value_target(angles_set)
except MoveItCommanderException as e:
pass #stupid bug in movegroupcommander wrapper throws an exception when trying to set joint angles
try:
self.arm.go()
return "successful home"
except:
return "failed to home"
# This callback function monitors the Joint Torques and stops the current execution if the Joint Torques exceed certain value
def monitorJointTorques(self, torques):
if abs(torques.joint1) > 1:
return
#self.emergency_stop() #Stop arm driver
#rospy.sleep(1.0)
#self.group.stop() #Stop moveit execution
# This callback function monitors the Joint Wrench and stops the current
# execution if the Joint Wrench exceeds certain value
def monitorToolWrench(self, wrenchStamped):
return
#toolwrench = abs(wrenchStamped.wrench.force.x**2 + wrenchStamped.wrench.force.y**2 + wrenchStamped.wrench.force.z**2)
##print toolwrench
#if toolwrench > 100:
# self.emergency_stop() # Stop arm driver
def move_fingers(self, finger1_pct, finger2_pct, finger3_pct):
finger_max_turn = 6800
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float((finger1_pct / 100.0) * finger_max_turn)
goal.fingers.finger2 = float((finger2_pct / 100.0) * finger_max_turn)
goal.fingers.finger3 = float((finger3_pct / 100.0) * finger_max_turn)
self.finger_action_client.send_goal(goal)
if self.finger_action_client.wait_for_result(rospy.Duration(5.0)):
return self.finger_action_client.get_result()
else:
self.finger_action_client.cancel_all_goals()
rospy.loginfo('the gripper action timed-out')
return None
def move_joint_angles(self, angle_set):
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angle_set[0]
goal.angles.joint2 = angle_set[1]
goal.angles.joint3 = angle_set[2]
goal.angles.joint4 = angle_set[3]
goal.angles.joint5 = angle_set[4]
goal.angles.joint6 = angle_set[5]
goal.angles.joint7 = angle_set[6]
self.joint_action_client.send_goal(goal)
if self.joint_action_client.wait_for_result(rospy.Duration(20.0)):
return self.joint_action_client.get_result()
else:
print(' the joint angle action timed-out')
self.joint_action_client.cancel_all_goals()
return None
def handle_move_block(self, message):
"""msg format: {id: int,
source: Point {x: float,y: float},
target: Point {x: float, y: float}
"""
print(message)
pick_x = message.source.x
pick_y = message.source.y
pick_x_threshold = message.source_x_tolerance
pick_y_threshold = message.source_y_tolerance
block_id = message.id
place_x = message.target.x
place_y = message.target.y
place_x_threshold = message.target_x_tolerance
place_y_threshold = message.target_y_tolerance
self.move_block(block_id, pick_x, pick_y, pick_x_threshold,
pick_y_threshold, place_x, place_y, place_x_threshold,
place_y_threshold, message.block_size)
def handle_pick_failure(self, exception):
rospy.loginfo("Pick failed, going home.")
self.open_gripper()
self.home_arm()
raise exception
def handle_place_failure(self, safe_zone, block_size, exception):
#should probably figure out if I'm holding the block first so it doesn't look weird
#figure out how to drop the block somewhere safe
#pass none and none if you are certain you haven't picked up a block yet
if safe_zone is None and block_size is None:
self.home_arm()
raise exception
rospy.loginfo("HANDLING PLACE FAILURE")
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(
safe_zone[0]['x'],
safe_zone[0]['y'],
safe_zone[1]['x_tolerance'],
safe_zone[1]['y_tolerance'],
current_block_state,
block_size,
num_attempts=1000)
try:
arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("panic arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
except Exception as e:
rospy.loginfo(
"ERROR: Cannot panic place the block! Get ready to catch it!")
self.open_gripper()
self.home_arm()
raise exception
def get_candidate_blocks(self, block_id, pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
return candidate_blocks
def move_block(self,
block_id,
pick_x,
pick_y,
pick_x_tolerance,
pick_y_tolerance,
place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
block_size=None,
safe_zone=None,
pick_tries=2,
place_tries=1,
point_at_block=True):
if block_size is None:
block_size = get_ros_param('DEFAULT_BLOCK_SIZE')
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
# check for a drop location before trying to pick, do this before checking source to prevent cases where we go for a block user
# moved while we are checking for a drop location
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
None, None,
Exception("no room in the target zone to drop the block"))
# here we are actually building a set of candidate blocks to pick
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
# select best block to pick and pick up
pick_location = None
if len(candidate_blocks) < 1:
raise Exception("no block of id " + str(block_id) +
" found within the source zone")
elif len(candidate_blocks) == 1:
pick_location = Point(candidate_blocks[0]['x'],
candidate_blocks[0]['y'], 0)
else:
pick_location = Point(*self.find_most_isolated_block(
candidate_blocks, current_block_state))
if point_at_block == True:
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.close_gripper()
self.point_at_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0))
self.point_at_block(location=Point(arm_drop_location[0],
arm_drop_location[1], 0))
self.home_arm()
except Exception as e:
rospy.loginfo(str(e))
rospy.loginfo("failed trying to point at block. giving up.")
self.home_arm()
self.move_block_server.set_succeeded(
MoveBlockResult(drop_location))
return
for i in range(pick_tries):
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.pick_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0),
check_grasp=True)
break
except Exception as e:
if i < pick_tries - 1:
rospy.loginfo("pick failed and trying again..." + str(e))
else:
rospy.loginfo("pick failed and out of attempts..." +
str(e))
self.handle_pick_failure(e)
if safe_zone == None:
if self.safe_zone == None:
safe_zone = [{
'x': pick_x,
'y': pick_y
}, {
'x_tolerance': pick_x_tolerance,
'y_tolerance': pick_y_tolerance
}]
else:
safe_zone = self.safe_zone
# calculate drop location
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
safe_zone, block_size,
Exception("no room in the target zone to drop the block"))
rospy.loginfo("tuio drop" + str(drop_location))
for i in range(place_tries):
try:
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
break
except Exception as e:
if i < place_tries - 1:
rospy.loginfo("place failed and trying again..." + str(e))
else:
rospy.loginfo("place failed and out of attempts..." +
str(e))
# check to see if we've defined a safe zone to drop the blocks
self.handle_place_failure(safe_zone, block_size, e)
# assume success if we made it this far
self.move_block_server.set_succeeded(MoveBlockResult(drop_location))
# check if a certain x, y position is within the bounds of another x,y position
@staticmethod
def check_block_bounds(x, y, x_origin, y_origin, x_threshold, y_threshold):
if x <= x_origin + x_threshold and x >= x_origin - x_threshold \
and y <= y_origin + y_threshold and y >= y_origin - y_threshold:
return True
return False
# calculate the best location to drop the block
def calculate_drop_location(self,
x,
y,
x_threshold,
y_threshold,
blocks,
block_size,
num_attempts=1000):
attempt = 0
x_original, y_original = x, y
while (attempt < num_attempts):
valid = True
for block in blocks:
if self.check_block_bounds(block['x'], block['y'], x, y,
0.8 * block_size, block_size):
valid = False
break
if valid:
return Point(x, y, 0)
else:
x = random.uniform(x_original - x_threshold,
x_original + x_threshold)
y = random.uniform(y_original - y_threshold,
y_original + y_threshold)
attempt += 1
#if none found in num_attempts, return none
return None
# candidates should have more than one element in it
@staticmethod
def find_most_isolated_block(candidates, all_blocks):
print(candidates)
min_distances = [] # tuples of candidate, distance to closest block
for candidate in candidates:
min_dist = -1
for block in all_blocks:
if block['x'] == candidate['x'] and block['y'] == candidate[
'y']:
continue
else:
dist = DaArmServer.block_dist(candidate, block)
if min_dist == -1 or dist < min_dist:
min_dist = dist
min_distances.append([candidate, min_dist])
most_isolated, _ = max(min_distances, key=lambda x: x[
1]) # get most isolated candidate, and min_distance
return most_isolated['x'], most_isolated['y'], 0
@staticmethod
def block_dist(block_1, block_2):
return np.sqrt((block_2['x'] - block_1['x'])**2 +
(block_2['y'] - block_1['y'])**2)
def update_finger_position(self, message):
self.finger_positions = [
message.finger1, message.finger2, message.finger3
]
def check_grasp(self):
closed_pos = 0.95 * 6800
distance_from_closed = 0
for fp in self.finger_positions:
distance_from_closed += (closed_pos - fp)**2
if np.sqrt(distance_from_closed
) > 130: #this is just some magic number for now
return True #if the fingers aren't fully closed, then grasp is good
else:
return False
def open_gripper(self, delay=0):
"""open the gripper ALL THE WAY, then delay
"""
if self.is_simulation is True:
self.gripper.set_named_target("Open")
self.gripper.go()
else:
try:
rospy.loginfo("Opening Gripper!!")
self.move_fingers(50, 50, 50)
except Exception as e:
rospy.loginfo("Caught it!!" + str(e))
rospy.sleep(delay)
def close_gripper(self, delay=0):
"""close the gripper ALL THE WAY, then delay
"""
# self.gripper.set_named_target("Close")
# self.gripper.go()
try:
rospy.loginfo("Closing Gripper!!")
self.move_fingers(95, 95, 95)
except Exception as e:
rospy.loginfo("Caught it!!" + str(e))
rospy.sleep(delay)
def handle_gesture(self, gesture):
# lookup the gesture from a table? or how about a message?
# one possibility, object that contains desired deltas in pos/orientation
# as well as specify the arm or gripper choice
pass
def handle_move_pose(self, message):
# takes a geometry_msgs/Pose message
self.move_arm_to_pose(message.target.position,
message.target.orientation,
action_server=self.move_pose_server)
self.move_pose_server.set_succeeded()
def check_plan_result(self, target_pose, cur_pose):
#we'll do a very lenient check, this is to find failures, not error
#also only checking position, not orientation
rospy.loginfo("checking pose:" + str(target_pose) + str(cur_pose))
if np.abs(target_pose.pose.position.x -
cur_pose.pose.position.x) > self.GOAL_TOLERANCE * 2:
print("x error too far")
return False
if np.abs(target_pose.pose.position.y -
cur_pose.pose.position.y) > self.GOAL_TOLERANCE * 2:
print("y error too far")
return False
if np.abs(target_pose.pose.position.z -
cur_pose.pose.position.z) > self.GOAL_TOLERANCE * 2:
print("z error too far")
return False
print("tolerable error")
return True
# expects cooridinates for position to be in arm space
def move_arm_to_pose(self,
position,
orientation,
delay=0.5,
waypoints=[],
corrections=4,
action_server=None):
for i in range(corrections + 1):
rospy.loginfo("TRY NUMBER " + str(i))
if len(waypoints) > 0 and i < 1:
# this is good for doing gestures
plan, fraction = self.arm.compute_cartesian_path(
waypoints, eef_step=0.01, jump_threshold=0.0)
else:
p = self.arm.get_current_pose()
p.pose.position = position
p.pose.orientation = orientation
rospy.loginfo("PLANNING TO " + str(p))
self.arm.set_pose_target(p)
last_traj_goal = self.last_joint_trajectory_goal
rospy.loginfo("EXECUTING!")
plan = self.arm.go(wait=False)
timeout = 5 / 0.001
while self.last_joint_trajectory_goal == last_traj_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Timeout waiting for kinova to accept movement goal."
))
rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
current_goal = self.last_joint_trajectory_goal
# then loop until a result for it gets published
timeout = 90 / 0.001
while self.last_joint_trajectory_result != current_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Motion took longer than 90 seconds. aborting..."))
if self.paused is True:
self.arm.stop() # cancel the moveit goals
return
rospy.loginfo("LEAVING THE WHILE LOOP")
# for i in range(corrections):
# rospy.loginfo("Correcting the pose")
# self.move_joint_angles(angle_set)
rospy.sleep(delay)
if (self.check_plan_result(p, self.arm.get_current_pose())):
break #we're there, no need to retry
#rospy.loginfo("OK GOT THROUGH THE PLANNING PHASE")
if False:
# # get the last pose to correct if desired
# ptPos = plan.joint_trajectory.points[-1].positions
# # print "=================================="
# # print "Last point of the current trajectory: "
# angle_set = list()
# for i in range(len(ptPos)):
# tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360
# angle_set.append(tempPos)
if action_server:
pass
#action_server.publish_feedback(CalibrateFeedback("Plan Found"))
last_traj_goal = self.last_joint_trajectory_goal
rospy.loginfo("EXECUTING!")
self.arm.execute(plan, wait=False)
# this is a bit naive, but I'm going to loop until a new trajectory goal gets published
timeout = 5 / 0.001
while self.last_joint_trajectory_goal == last_traj_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Timeout waiting for kinova to accept movement goal."
))
rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
current_goal = self.last_joint_trajectory_goal
# then loop until a result for it gets published
timeout = 15 / 0.001
while self.last_joint_trajectory_result != current_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Motion took longer than 15 seconds. aborting..."))
if self.paused is True:
self.arm.stop() # cancel the moveit goals
return
rospy.loginfo("LEAVING THE WHILE LOOP")
# for i in range(corrections):
# rospy.loginfo("Correcting the pose")
# self.move_joint_angles(angle_set)
rospy.sleep(delay)
else:
if action_server:
#action_server.publish_feedback(CalibrateFeedback("Planning Failed"))
pass
# Let's have the caller handle sequences instead.
# def handle_move_sequence(self, message):
# # if the move fails, do we cancel the sequence
# cancellable = message.cancellable
# moves = message.moves
# for move in moves:
# try:
# self.handle_move_block(move)
# except Exception:
# if cancellable:
# rospy.loginfo("Part of move failed, cancelling the rest.")
# break
def update_move_group_state(self, message):
rospy.loginfo(message.feedback.state)
self.move_group_state = message.feedback.state
def update_move_group_status(self, message):
if message.status_list:
#rospy.loginfo("MoveGroup Status for "+str(message.status_list[0].goal_id.id)+": "+str(message.status_list[0].status))
self.move_group_status = message.status_list[0].status
def update_joint_trajectory_state(self, message):
# print(message.status_list)
if len(message.status_list) > 0:
self.joint_trajectory_state = message.status_list[0].status
else:
self.joint_trajectory_state = 0
def update_joint_trajectory_goal(self, message):
#print("goalisasis", message.goal_id.id)
self.last_joint_trajectory_goal = message.goal_id.id
def update_joint_trajectory_result(self, message):
#print("resultisasis", message.status.goal_id.id)
self.last_joint_trajectory_result = message.status.goal_id.id
def get_grasp_orientation(self, position):
#return Quaternion(0, 0, 1/np.sqrt(2), 1/np.sqrt(2))
return Quaternion(-0.707388, -0.706825, 0.0005629, 0.0005633)
def update_joints(self, joints):
self.joint_angles = [
joints.joint1, joints.joint2, joints.joint3, joints.joint4,
joints.joint5, joints.joint6, joints.joint7
]
def move_z_relative(self, distance):
p = self.arm.get_current_pose()
p.pose.position.z += distance
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def move_z_absolute(self, height):
p = self.arm.get_current_pose()
p.pose.position.z = height
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def pick_block(self,
location,
check_grasp=False,
retry_attempts=0,
delay=0,
action_server=None):
# go to a spot and pick up a block
# if check_grasp is true, it will check torque on gripper and retry or fail if not holding
# open the gripper
# print('Position: ', position)
self.open_gripper()
position = Point(location.x, location.y, self.cruising_height)
orientation = self.get_grasp_orientation(position)
try:
self.raise_table()
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
position = Point(location.x, location.y, self.grasp_height)
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.crusing_height)
self.raise_table()
raise (
e
) #problem because sometimes we get exception e.g. if we're already there
# and it will skip the close if so.
if action_server:
action_server.publish_feedback()
self.close_gripper()
self.move_z_absolute(self.cruising_height)
#try to wait until we're up to check grasp
rospy.sleep(0.5)
if check_grasp is True:
if (self.check_grasp() is False):
print("Grasp failed!")
self.raise_table()
raise (Exception("grasp failed!"))
# for now, but we want to check finger torques
# for i in range(retry_attempts):
# self.move_z(0.3)
self.raise_table()
rospy.sleep(delay)
def place_block(self,
location,
check_grasp=False,
delay=0,
action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
# print('Position: ', position)
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct
self.move_z_absolute(self.cruising_height)
position = Point(location.x, location.y, self.cruising_height)
rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
str(self.drop_height))
orientation = self.get_grasp_orientation(position)
try:
self.raise_table(
) # only do this as a check in case it isn't already raised
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
self.move_z_absolute(self.drop_height)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.cruising_height)
self.raise_table()
raise (e)
if action_server:
action_server.publish_feedback()
self.open_gripper()
self.move_z_absolute(self.cruising_height)
self.raise_table()
self.close_gripper()
def point_at_block(self, location, delay=0, action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
# print('Position: ', position)
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct
self.move_z_absolute(self.cruising_height)
position = Point(location.x, location.y, self.cruising_height)
rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
str(self.drop_height))
orientation = self.get_grasp_orientation(position)
try:
self.raise_table(
) # only do this as a check in case it isn't already raised
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
self.move_z_absolute(self.pointing_height)
self.move_z_absolute(self.cruising_height)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.cruising_height)
self.raise_table()
raise (e)
if action_server:
action_server.publish_feedback()
self.raise_table()
def stop_motion(self, home=False, pause=False):
rospy.loginfo("STOPPING the ARM")
# cancel the moveit_trajectory
# self.arm.stop()
# call the emergency stop on kinova
self.emergency_stop()
# rospy.sleep(0.5)
if pause is True:
self.paused = True
if home is True:
# self.restart_arm()
self.home_arm()
def calibrate(self, message):
print("calibrating ", message)
self.place_calibration_block()
rospy.sleep(5) # five seconds to correct the drop if it bounced, etc.
print("moving on...")
self.calibration_server.publish_feedback(
CalibrateFeedback("getting the coordinates"))
params = self.record_calibration_params()
self.set_arm_calibration_params(params[0], params[1])
self.calibration_publisher.publish(
CalibrationParams(params[0], params[1]))
self.calibration_server.set_succeeded(CalibrateResult(params))
def set_arm_calibration_params(self, arm_x_min, arm_y_min):
rospy.set_param("ARM_X_MIN", arm_x_min)
rospy.set_param("ARM_Y_MIN", arm_y_min)
def place_calibration_block(self):
# start by homing the arm (kinova home)
self.calibration_server.publish_feedback(CalibrateFeedback("homing"))
self.home_arm_kinova()
# go to grab a block from a human
self.open_gripper()
self.close_gripper()
self.open_gripper(4)
self.close_gripper(2)
# move to a predetermined spot
self.calibration_server.publish_feedback(
CalibrateFeedback("moving to drop"))
try:
self.move_arm_to_pose(Point(0.4, -0.4, 0.1),
Quaternion(1, 0, 0, 0),
corrections=0)
except Exception as e:
rospy.loginfo("THIS IS TH PRoblem" + str(e))
# drop the block
self.open_gripper()
self.calibration_server.publish_feedback(
CalibrateFeedback("dropped the block"))
calibration_block = {'x': 0.4, 'y': -0.4, 'id': 0}
calibration_action_belief = {
"action": "add",
"block": calibration_block
}
self.action_belief_publisher.publish(
String(json.dumps(calibration_action_belief)))
rospy.loginfo("published arm belief")
return
def record_calibration_params(self):
""" Call the block_tracker service to get the current table config.
Find the calibration block and set the appropriate params.
"""
# make sure we know the dimensions of the table before we start
# fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y
tuio_bounds = get_tuio_bounds()
arm_bounds = get_arm_bounds(calibrate=False)
try:
block_state = json.loads(self.get_block_state("tuio").tui_state)
except rospy.ServiceException as e:
# self.calibration_server.set_aborted()
raise (ValueError("Failed getting block state to calibrate: " +
str(e)))
if len(block_state) != 1:
# self.calibration_server.set_aborted()
raise (ValueError(
"Failed calibration, either couldn't find block or > 1 block on TUI!"
))
# if we made it here, let's continue!
# start by comparing the reported tuio coords where we dropped the block
# with the arm's localization of the end-effector that dropped it
# (we assume that the block fell straight below the drop point)
drop_pose = self.arm.get_current_pose()
end_effector_x = drop_pose.pose.position.x
end_effector_y = drop_pose.pose.position.y
block_tuio_x = block_state[0]['x']
block_tuio_y = block_state[0]['y']
x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y,
tuio_bounds)
arm_x_min = end_effector_x - x_ratio * arm_bounds["x_dist"]
arm_y_min = end_effector_y - y_ratio * arm_bounds["y_dist"]
return [arm_x_min, arm_y_min]
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
self.obj_att = None
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = target_pose
initial_pose.header.frame_id = 'gazebo_world'
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
# M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
# M1[0,3] = target_pose.pose.position.x
# M1[1,3] = target_pose.pose.position.y
# M1[2,3] = target_pose.pose.position.z
# M2 = transformations.euler_matrix(0, 1.57, 0)
# M2[0,3] = 0.0 # offset about x
# M2[1,3] = 0.0 # about y
# M2[2,3] = 0.25 # about z
# T = np.dot(M1, M2)
# pre_grasping = deepcopy(target_pose)
# pre_grasping.pose.position.x = T[0,3]
# pre_grasping.pose.position.y = T[1,3]
# pre_grasping.pose.position.z = T[2,3]
# quat = transformations.quaternion_from_matrix(T)
# pre_grasping.pose.orientation.x = quat[0]
# pre_grasping.pose.orientation.y = quat[1]
# pre_grasping.pose.orientation.z = quat[2]
# pre_grasping.pose.orientation.w = quat[3]
# pre_grasping.header.frame_id = 'gazebo_world'
# self.plan_exec(pre_grasping)
################# GENERATE GRASPS ###################
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Track success/failure and number of attempts for pick operation
success = False
n_attempts = 0
grasps = self.grasp_generator(initial_pose)
possible_grasps = []
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
possible_grasps.append(grasp.pose)
rospy.sleep(0.2)
#print possible_grasps
self.right_arm.pick(target_id, grasps)
# target_name = target_id
# group_name = GROUP_NAME_ARM
# end_effector = GROUP_NAME_GRIPPER
# attached_object_touch_links = ['r_forearm_link']
# #print (target_name, group_name, end_effector)
# PickupGoal(target_name, group_name ,end_effector, possible_grasps )
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def grasp_generator(self, initial_pose):
# Pitch angles to try
pitch_vals = [1, 1.57,0, 1,2 ]
# Yaw angles to try
yaw_vals = [0]#, 1.57, -1.57]
# A list to hold the grasps
grasps = []
g = PoseStamped()
g.header.frame_id = REFERENCE_FRAME
g.pose = initial_pose.pose
#g.pose.position.z += 0.18
# Generate a grasp for each pitch and yaw angle
for y in yaw_vals:
for p in pitch_vals:
# Create a quaternion from the Euler angles
q = transformations.quaternion_from_euler(0, p, y)
# Set the grasp pose orientation accordingly
g.pose.orientation.x = q[0]
g.pose.orientation.y = q[1]
g.pose.orientation.z = q[2]
g.pose.orientation.w = q[3]
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
next_call = time.time()
while True:
next_call = next_call+1
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
# obstacle1_id = 'obstacle1'
# self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
# obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
if self.obj_att is None:
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
# obstacle1_pose = PoseStamped()
# obstacle1_pose.header.frame_id = REFERENCE_FRAME
# obstacle1_pose.pose = self.pwh.pose[self.o1id]
# # Add the target object to the scene
# scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
time.sleep(next_call - time.time())
#threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
class WarehousePlanner(object):
def __init__(self):
rospy.init_node('moveit_warehouse_planner', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(2)
group_id = rospy.get_param("~arm_group_name")
self.eef_step = rospy.get_param("~eef_step", 0.01)
self.jump_threshold = rospy.get_param("~jump_threshold", 1000)
self.group = MoveGroupCommander(group_id)
# self._add_ground()
self._robot_name = self.robot._r.get_robot_name()
self._robot_link = self.group.get_end_effector_link()
self._robot_frame = self.group.get_pose_reference_frame()
self._min_wp_fraction = rospy.get_param("~min_waypoint_fraction", 0.9)
rospy.sleep(4)
rospy.loginfo("Waiting for warehouse services...")
rospy.wait_for_service('moveit_warehouse_services/list_robot_states')
rospy.wait_for_service('moveit_warehouse_services/get_robot_state')
rospy.wait_for_service('moveit_warehouse_services/has_robot_state')
rospy.wait_for_service('/compute_fk')
self._list_states = rospy.ServiceProxy(
'moveit_warehouse_services/list_robot_states', ListStates)
self._get_state = rospy.ServiceProxy(
'moveit_warehouse_services/get_robot_state', GetState)
self._has_state = rospy.ServiceProxy(
'moveit_warehouse_services/has_robot_state', HasState)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
rospy.loginfo("Service proxies connected")
self._tr_frm_list_srv = rospy.Service('plan_trajectory_from_list',
PlanTrajectoryFromList,
self._plan_from_list_cb)
self._tr_frm_prfx_srv = rospy.Service('plan_trajectory_from_prefix',
PlanTrajectoryFromPrefix,
self._plan_from_prefix_cb)
self._execute_tr_srv = rospy.Service('execute_planned_trajectory',
ExecutePlannedTrajectory,
self._execute_plan_cb)
self.__plan = None
def get_waypoint_names_by_prefix(self, prefix):
regex = "^" + str(prefix) + ".*"
waypoint_names = self._list_states(regex, self._robot_name).states
return waypoint_names
def get_pose_from_state(self, state):
header = Header()
fk_link_names = [self._robot_link]
header.frame_id = self._robot_frame
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0].pose
def get_cartesian_waypoints(self, waypoint_names):
waypoints = []
waypoints.append(self.group.get_current_pose().pose)
for name in waypoint_names:
if self._has_state(name, self._robot_name).exists:
robot_state = self._get_state(name, "").state
waypoints.append(self.get_pose_from_state(robot_state))
else:
rospy.logerr("Waypoint %s doesn't exist for robot %s.", name,
self._robot_name)
return waypoints
def _add_ground(self):
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below the dome
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.01))
rospy.sleep(1)
def plan_from_filter(self, prefix):
waypoint_names = self.get_waypoint_names_by_prefix(prefix)
waypoint_names.sort()
if 0 == len(waypoint_names):
rospy.logerr(
"No waypoints found for robot %s with prefix %s. " +
"Can't make trajectory :(", self._robot_name, str(prefix))
return False
rospy.loginfo(
"Creating trajectory with %d waypoints selected by " +
"prefix %s.", len(waypoint_names), str(prefix))
return self.plan_from_list(waypoint_names)
def plan_from_list(self, waypoint_names):
self.group.clear_pose_targets()
waypoints = self.get_cartesian_waypoints(waypoint_names)
if len(waypoints) != len(waypoint_names) + 1:
# +1 because current position is used as first waypiont.
rospy.logerr("Not all waypoints existed, not executing.")
return False
(plan,
fraction) = self.group.compute_cartesian_path(waypoints,
self.eef_step,
self.jump_threshold)
if fraction < self._min_wp_fraction:
rospy.logerr(
"Only managed to generate trajectory through" +
"%f of waypoints. Not executing", fraction)
return False
self.__plan = plan
return True
def _plan_from_list_cb(self, request):
# check all exist
self.__plan = None
rospy.loginfo("Creating trajectory from points given: %s",
",".join(request.waypoint_names))
return self.plan_from_list(request.waypoint_names)
def _plan_from_prefix_cb(self, request):
self.__plan = None
rospy.loginfo("Creating trajectory from points filtered by prefix %s",
request.prefix)
return self.plan_from_filter(request.prefix)
def _execute_plan_cb(self, request):
if self.__plan is None:
rospy.logerr("No plan stored!")
return False
rospy.loginfo("Executing stored plan")
response = self.group.execute(self.__plan)
self.__plan = None
return response
class Arm(object):
def __init__(self, move_group):
# create move group
self.group = MoveGroupCommander(move_group)
def get_pose(self):
"""
Get current pose for specified move group
Returns
ros msg (geometry_msgs.msg._PoseStamped.PoseStamped) - A message
specifying the pose of the move group as position (XYZ) and
orientation (xyzw).
"""
return self.group.get_current_pose().pose
def get_joint_values(self):
"""
Get joint values for specified move group
Returns
joint value (list) - A list of joint values in radians
"""
return self.group.get_current_joint_values()
def get_planning_frame(self):
"""
Get planning frame of move group
"""
return self.group.get_planning_frame()
def clear_pose_targets(self, move_group):
"""
Clear target pose for planning
"""
self.group.clear_pose_targets()
def plan_to_pose(self, pose):
"""
Plan to a given pose for the end-effector
Args
pose (msg): geometry_msgs.msg.Pose
"""
# Clear old pose targets
self.group.clear_pose_targets()
# Set target pose
self.group.set_pose_target(pose)
numTries = 0
while numTries < 5:
plan = self.group.plan()
numTries += 1
if len(plan.joint_trajectory.points) > 0:
print('succeeded in %d tries' % numTries)
return True
print('Planning failed')
return False
def plan_to_joints(self, joint_angles):
"""Plan to a given joint configuration
Args
joint_angles (list of floats): list of len 7 of joint angles in radians
Returns
True if planning succeeds, False if not
"""
# Clear old pose targets
self.group.clear_pose_targets()
# Set Joint configuration target
self.group.set_joint_value_target(joint_angles)
numTries = 0
while numTries < 5:
plan = group.plan()
numTries += 1
if len(plan.joint_trajectory.points) > 0:
print('succeeded in %d tries' % numTries)
return True
print("Planning failed")
return False
def move_to_joints(self, joint_angles):
"""
Move to specified joint configuration
Args
joint_angles (list of floats): list of len 7 of joint angles in radians
blocking (bool): If True,
Returns
True if planning succeeds, False if not
"""
# plan
plan = self.plan_to_joints(group.get_name(), joint_angles)
# move to joint configuration
if plan:
group.go(wait=True)
return plan
def move_to_pose(self, pose):
"""
Move to specified joint configuration
Args
pose (msg): geometry_msgs.msg.Pose
joint_angles (list of floats): list of len 7 of joint angles in radians
blocking (bool): If True,
Returns
True if planning succeeds, False if not
"""
# plan
plan = self.plan_to_pose(pose)
if plan:
group.go(wait=blocking)
group.stop()
group.clear_pose_targets()
return
def _change_planner(self, planner):
planners = [
"SBLkConfigDefault", "ESTkConfigDefault", "LBKPIECEkConfigDefault",
"BKPIECEkConfigDefault", "KPIECEkConfigDefault",
"RRTkConfigDefault", "RRTConnectkConfigDefault",
"RRTstarkConfigDefault", "TRRTkConfigDefault", "PRMkConfigDefault",
"PRMstarkConfigDefault"
]
if planner in planners:
self.group.set_planner_id(planner)
return
def create_subscriber(self, topic, message_type):
return
