def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_test_cartesian', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
rospy.sleep(2)
pose = PoseStamped().pose
# same orientation for all
q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
pose.orientation = Quaternion(*q)
i = 1
while i <= 10:
waypoints = list()
j = 1
while j <= 5:
# random pose
rand_pose = arm.get_random_pose(arm.get_end_effector_link())
pose.position.x = rand_pose.pose.position.x
pose.position.y = rand_pose.pose.position.y
pose.position.z = rand_pose.pose.position.z
waypoints.append(copy.deepcopy(pose))
j += 1
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "====== waypoints created ======="
# print waypoints
# ======= show cartesian path ====== #
arm.go()
rospy.sleep(10)
i += 1
print "========= end ========="
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_test_position', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
# max planning time
arm.set_planning_time(10)
# start pose
arm.set_start_state_to_current_state()
end_effector = arm.get_end_effector_link()
rospy.sleep(1)
print "======== create 100 new goal position ========"
start_pose = PoseStamped()
start_pose.header.frame_id = arm.get_planning_frame()
i = 1
while i <= 1000:
# random position
start_pose = arm.get_random_pose(end_effector)
q = quaternion_from_euler(0.0, 0.0, 0.0)
start_pose.pose.orientation = Quaternion(*q)
print "====== move to position", i, "======="
# KDL
arm.set_joint_value_target(start_pose, True)
# IK Fast
# arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y,
# start_pose.pose.position.z], end_effector)
i += 1
arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class Planner(object):
move_group = None
goals = None
jspub = None
namespace = None
# These will eventually go to model objects
robot_data = {
'group_name': 'right_arm_and_torso',
'eef_link': 'r_wrist_joint_link'
}
# Current state of the 'session' (right now, only one)
current_scene = None
status = None
link_poses = None
def __init__(self):
rospy.init_node('moveit_web',disable_signals=True)
self.jspub = rospy.Publisher('/update_joint_states',JointState)
self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
# Give time for subscribers to connect to the publisher
rospy.sleep(1)
self.goals = []
# HACK: Synthesize a valid initial joint configuration for PR2
initial_joint_state = JointState()
initial_joint_state.name = ['r_elbow_flex_joint']
initial_joint_state.position = [-0.1]
self.jspub.publish(initial_joint_state)
# Create group we'll use all along this demo
# self.move_group = MoveGroupCommander('right_arm_and_torso')
self.move_group = MoveGroupCommander(self.robot_data['group_name'])
self._move_group = self.move_group._g
self.ps = PlanningSceneInterface()
self.status = {'text':'ready to plan','ready':True}
def get_scene(self):
return self.current_scene
def set_scene(self, scene):
self.current_scene = scene
psw = PlanningSceneWorld()
for co_json in scene['objects']:
# TODO: Fix orientation by using proper quaternions on the client
pose = self._make_pose(co_json['pose'])
# TODO: Decide what to do with STL vs. Collada. The client has a Collada
# loader but the PlanningSceneInterface can only deal with STL.
# TODO: Proper mapping between filenames and URLs
# filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
co = self.ps.make_mesh(co_json['name'], pose, filename)
psw.collision_objects.append(co)
self.psw_pub.publish(psw)
def get_link_poses(self):
if self.link_poses is None:
self.link_poses = self._move_group.get_link_poses_compressed()
return self.link_poses
# Create link back to socket.io namespace to allow emitting information
def set_socket(self, namespace):
self.namespace = namespace
def emit(self, event, data=None):
if self.namespace:
self.namespace.emit(event, data)
def emit_new_goal(self, pose):
self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose'])
def set_random_goal(self):
goal_pose = self.move_group.get_random_pose()
# goal_pose = self.move_group.get_random_pose('base_footprint')
self.emit_new_goal(goal_pose)
def _make_pose(self, json_pose):
pose = PoseStamped()
pose.header.frame_id = "odom_combined"
pp = json_pose['position']
pose.pose.position.x = pp['x']
pose.pose.position.y = pp['y']
pose.pose.position.z = pp['z']
# TODO: Orientation is not working. See about
# properly using Quaternions everywhere
pp = json_pose['orientation']
pose.pose.orientation.x = pp['x']
pose.pose.orientation.y = pp['y']
pose.pose.orientation.z = pp['z']
pose.pose.orientation.w = pp['w']
return pose
def plan_to_poses(self, poses):
goal_pose = self._make_pose(poses[0])
self.move_group.set_pose_target(goal_pose)
# self.move_group.set_pose_target(goal_pose,'base_footprint')
self.emit('status',{'text':'Starting to plan'})
trajectory = self.move_group.plan()
if trajectory is None or len(trajectory.joint_trajectory.joint_names) == 0:
self.status = {'reachable':False,'text':'Ready to plan','ready':True}
self.emit('status', self.status)
else:
self.status = {'reachable':True,'text':'Rendering trajectory'}
self.emit('status', self.status)
self.publish_trajectory(trajectory)
def publish_goal_position(self, trajectory):
self.publish_position(self, trajectory, -1)
def publish_position(self, trajectory, step):
jsmsg = JointState()
jsmsg.name = trajectory.joint_trajectory.joint_names
jsmsg.position = trajectory.joint_trajectory.points[step].positions
self.jspub.publish(jsmsg)
def publish_trajectory(self, trajectory):
cur_time = 0.0
acceleration = 4.0
for i in range(len(trajectory.joint_trajectory.points)):
point = trajectory.joint_trajectory.points[i]
gevent.sleep((point.time_from_start - cur_time)/acceleration)
cur_time = point.time_from_start
# self.publish_position(trajectory, i)
# TODO: Only say "True" to update state on the last step of the trajectory
new_poses = self._move_group.update_robot_state(trajectory.joint_trajectory.joint_names,
trajectory.joint_trajectory.points[i].positions, True)
self.link_poses = new_poses
self.emit('link_poses', new_poses)
self.status = {'text':'Ready to plan','ready':True}
self.emit('status', self.status)
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 Planner(object):
move_group = None
goals = None
jspub = None
namespace = None
# These will eventually go to model objects
robot_data = {
'group_name': 'right_arm_and_torso',
'eef_link': 'r_wrist_joint_link'
}
# Current state of the 'session' (right now, only one)
current_scene = None
status = None
link_poses = None
def __init__(self):
rospy.init_node('moveit_web', disable_signals=True)
self.jspub = rospy.Publisher('/update_joint_states', JointState)
self.psw_pub = rospy.Publisher('/planning_scene_world',
PlanningSceneWorld)
# Give time for subscribers to connect to the publisher
rospy.sleep(1)
self.goals = []
# HACK: Synthesize a valid initial joint configuration for PR2
initial_joint_state = JointState()
initial_joint_state.name = ['r_elbow_flex_joint']
initial_joint_state.position = [-0.1]
self.jspub.publish(initial_joint_state)
# Create group we'll use all along this demo
# self.move_group = MoveGroupCommander('right_arm_and_torso')
self.move_group = MoveGroupCommander(self.robot_data['group_name'])
self._move_group = self.move_group._g
self.ps = PlanningSceneInterface()
self.status = {'text': 'ready to plan', 'ready': True}
def get_scene(self):
return self.current_scene
def set_scene(self, scene):
self.current_scene = scene
psw = PlanningSceneWorld()
for co_json in scene['objects']:
# TODO: Fix orientation by using proper quaternions on the client
pose = self._make_pose(co_json['pose'])
# TODO: Decide what to do with STL vs. Collada. The client has a Collada
# loader but the PlanningSceneInterface can only deal with STL.
# TODO: Proper mapping between filenames and URLs
# filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
co = self.ps.make_mesh(co_json['name'], pose, filename)
psw.collision_objects.append(co)
self.psw_pub.publish(psw)
def get_link_poses(self):
if self.link_poses is None:
self.link_poses = self._move_group.get_link_poses_compressed()
return self.link_poses
# Create link back to socket.io namespace to allow emitting information
def set_socket(self, namespace):
self.namespace = namespace
def emit(self, event, data=None):
if self.namespace:
self.namespace.emit(event, data)
def emit_new_goal(self, pose):
self.emit(
'target_pose',
message_converter.convert_ros_message_to_dictionary(pose)['pose'])
def set_random_goal(self):
goal_pose = self.move_group.get_random_pose()
# goal_pose = self.move_group.get_random_pose('base_footprint')
self.emit_new_goal(goal_pose)
def _make_pose(self, json_pose):
pose = PoseStamped()
pose.header.frame_id = "odom_combined"
pp = json_pose['position']
pose.pose.position.x = pp['x']
pose.pose.position.y = pp['y']
pose.pose.position.z = pp['z']
# TODO: Orientation is not working. See about
# properly using Quaternions everywhere
pp = json_pose['orientation']
pose.pose.orientation.x = pp['x']
pose.pose.orientation.y = pp['y']
pose.pose.orientation.z = pp['z']
pose.pose.orientation.w = pp['w']
return pose
def plan_to_poses(self, poses):
goal_pose = self._make_pose(poses[0])
self.move_group.set_pose_target(goal_pose)
# self.move_group.set_pose_target(goal_pose,'base_footprint')
self.emit('status', {'text': 'Starting to plan'})
trajectory = self.move_group.plan()
if trajectory is None or len(
trajectory.joint_trajectory.joint_names) == 0:
self.status = {
'reachable': False,
'text': 'Ready to plan',
'ready': True
}
self.emit('status', self.status)
else:
self.status = {'reachable': True, 'text': 'Rendering trajectory'}
self.emit('status', self.status)
self.publish_trajectory(trajectory)
def publish_goal_position(self, trajectory):
self.publish_position(self, trajectory, -1)
def publish_position(self, trajectory, step):
jsmsg = JointState()
jsmsg.name = trajectory.joint_trajectory.joint_names
jsmsg.position = trajectory.joint_trajectory.points[step].positions
self.jspub.publish(jsmsg)
def publish_trajectory(self, trajectory):
cur_time = 0.0
acceleration = 4.0
for i in range(len(trajectory.joint_trajectory.points)):
point = trajectory.joint_trajectory.points[i]
gevent.sleep((point.time_from_start - cur_time) / acceleration)
cur_time = point.time_from_start
# self.publish_position(trajectory, i)
# TODO: Only say "True" to update state on the last step of the trajectory
new_poses = self._move_group.update_robot_state(
trajectory.joint_trajectory.joint_names,
trajectory.joint_trajectory.points[i].positions, True)
self.link_poses = new_poses
self.emit('link_poses', new_poses)
self.status = {'text': 'Ready to plan', 'ready': True}
self.emit('status', self.status)
def main():
rospy.init_node(sys.argv[1])
total_envs = 10
total_targets = int(
sys.argv[2]) #total number of collision free targets to save
limb = baxter_interface.Limb('right')
def_angles = limb.joint_angles()
limb.set_joint_position_speed(0.65)
#robot_state_collision_pub = rospy.Publisher('/robot_collision_state', DisplayRobotState, queue_size=0)
#scene = PlanningSceneInterface()
#scene._scene_pub = rospy.Publisher('planning_scene',
# PlanningScene,
# queue_size=0)
robot = RobotCommander()
group = MoveGroupCommander("right_arm")
rs_man = RobotState()
sv = StateValidity()
#set_environment(robot, scene)
masterModifier = ShelfSceneModifier()
#sceneModifier = PlanningSceneModifier(masterModifier.obstacles)
#sceneModifier.setup_scene(scene, robot, group)
robot_state = robot.get_current_state()
print(robot_state)
rs_man = RobotState() # constructed manually for comparison
rs_man.joint_state.name = robot_state.joint_state.name
filler_robot_state = list(robot_state.joint_state.position)
done = False
masterEnvDict = {}
envFileName = "testEnvironments_test1.pkl"
x_bounds = [[0.89, 1.13], [
-0.2, 1.13
]] #0 is the right half of the right table, 1 is the right table
y_bounds = [[-0.66, 0], [-0.9, -0.66]]
x_ranges = [
max(x_bounds[0]) - min(x_bounds[0]),
max(x_bounds[1]) - min(x_bounds[1])
]
y_ranges = [
max(y_bounds[0]) - min(y_bounds[0]),
max(y_bounds[1]) - min(y_bounds[1])
]
xy_bounds_dict = {
'x': x_bounds,
'y': y_bounds,
'x_r': x_ranges,
'y_r': y_ranges
}
iter_num = 0
#sceneModifier.delete_obstacles()
while (not rospy.is_shutdown() and not done):
for i_env in range(total_envs):
new_pose = masterModifier.permute_obstacles()
key_name = 'TrainEnv_' + str(i_env)
#sceneModifier.permute_obstacles(new_pose)
masterEnvDict[key_name] = {}
masterEnvDict[key_name]['ObsPoses'] = {}
masterEnvDict[key_name]['Targets'] = {}
masterEnvDict[key_name]['Targets']['Pose'] = []
masterEnvDict[key_name]['Targets']['Joints'] = []
for i_target in range(total_targets):
pose = sample_from_boundary(xy_bounds_dict)
check_pose = group.get_random_pose()
check_pose.pose.position.x = pose[0]
check_pose.pose.position.y = pose[1]
check_pose.pose.position.z = pose[2]
joint_angles = ik_test(check_pose)[1]
filler_robot_state[10:17] = moveit_scrambler(
joint_angles.values())
rs_man.joint_state.position = tuple(filler_robot_state)
while (ik_test(check_pose)[0]
or not sv.getStateValidity(rs_man,
group_name="right_arm")):
pose = sample_from_boundary(xy_bounds_dict)
check_pose = group.get_random_pose()
check_pose.pose.position.x = pose[0]
check_pose.pose.position.y = pose[1]
check_pose.pose.position.z = pose[2]
if (not ik_test(check_pose)[0]):
joint_angles = ik_test(check_pose)[1]
filler_robot_state[10:17] = moveit_scrambler(
joint_angles.values())
rs_man.joint_state.position = tuple(filler_robot_state)
joint_angles = ik_test(check_pose)[1]
masterEnvDict[key_name]['Targets']['Pose'].append(check_pose)
masterEnvDict[key_name]['Targets']['Joints'].append(
joint_angles)
#sceneModifier.delete_obstacles()
masterEnvDict[key_name]['ObsPoses'] = new_pose
iter_num += 1
with open(envFileName, "wb") as env_f:
pickle.dump(masterEnvDict, env_f)
print("Done saving... exiting loop \n\n\n")
done = True
mg = MoveGroupCommander('right_arm_and_torso')
p = mg.get_current_pose()
print "Start pose:"
print p
p.pose.position.x += 0.3
#ps = PlanningSceneInterface()
#psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
#rospy.sleep(1)
#co = ps.make_sphere("test_co", p, 0.02)
#psw = PlanningSceneWorld()
#psw.collision_objects.append(co)
#psw_pub.publish(psw)
# ps.remove_world_object("test_sphere")
# ps.add_sphere("test_sphere", p, 0.1)
# rospy.sleep(1)
# ps.add_sphere("test_sphere", p, 0.1)
#p.pose.position.x += 0.3
print "End pose:"
print p
mg.set_pose_target(mg.get_random_pose())
traj = mg.plan()
print traj
