def MoveArmTo(robot,goal,planner):
try:
orpy.RaveSetDebugLevel(orpy.DebugLevel.Verbose)
with env:
robot.SetActiveDOFs(range(5))
traj = planner.MoveActiveJoints(goal=goal,maxiter=5000,steplength=0.001,maxtries=2,execute=False,outputtrajobj=True, jitter=-0.00025)
robot.GetController().SetPath(traj)
while not robot.GetController().IsDone():
time.sleep(0.01)
finally:
orpy.RaveSetDebugLevel(orpy.DebugLevel.Info)
def MoveBaseTo(robot,xyyaw,planner,skip_waypoints=False):
print "move"
print robot
print xyyaw
try:
orpy.RaveSetDebugLevel(orpy.DebugLevel.Verbose)
robot.GetEnv().GetKinBody('floor').Enable(False)
current = robot.GetTransform()
currentxyyaw = np.array([current[0,3],current[1,3],np.arctan2(current[1,0],current[0,0])])
if np.linalg.norm(currentxyyaw-xyyaw) < 0.0001:
print robot.GetName(),' already at goal. Not moving.'
return
with env:
robot.SetActiveDOFs([],orpy.DOFAffine.X|orpy.DOFAffine.Y|orpy.DOFAffine.RotationAxis,[0,0,1])
traj = planner.MoveActiveJoints(goal=xyyaw,maxiter=5000,steplength=0.001,maxtries=2,execute=False,outputtrajobj=True, jitter=-0.00025)
print 'trajectory'
print traj
if skip_waypoints:
traj.Remove(1,traj.GetNumWaypoints()-1)
robot.GetController().SetPath(traj)
while not robot.GetController().IsDone():
time.sleep(0.01)
except Exception, e:
print str(e)
c = raw_input('IPython shell?(y/n)')
if c == 'y':
IPython.embed()
raise e
def __init__(self, args):
# Initialize values
self.busy = False
self.topic_name = args.topic
self.file_name = args.file_name
self.octomap_resolution = str(args.resolution)
self.octomap_range = str(args.range)
self.octomap_frame = args.frame
# env_name = '' # TODO:
# robot_name = '' # TODO:
# obstacle = '' # TODO:
self.durations = []
# Initialize OpenRAVE environment
self.env = orpy.Environment()
self.env.SetViewer('qtcoin')
orpy.RaveSetDebugLevel(orpy.DebugLevel.Error)
# self.env.Load(env_name)
# Initialize ROS subscribers and publisher
self.sub_octomap = rospy.Subscriber('/occupied_cells_vis_array',
MarkerArray,
callback=self.process_octomap,
queue_size=1)
# Initialize octomap plugin
orpy.RaveLoadPlugin("or_octomap") # Load octomap plugin
self.sensor_server = orpy.RaveCreateSensorSystem(self.env, "or_octomap")
self.sensor_server.SendCommand("Enable") # Enable octomap thread
self.sensor_server.SendCommand("ResetTopic " +
self.topic_name) # Reset the registered pcl topic
self.sensor_server.SendCommand("ResetResolution " +
self.octomap_resolution) # Reset the octomap resolution
self.sensor_server.SendCommand("ResetRange " +
self.octomap_range) # Reset the octomap max range
self.sensor_server.SendCommand("ResetFrameID " +
self.octomap_frame) # Reset the octomap frame
self.time_start = time.time()
def setUp(self):
import openravepy
if not self.is_setup:
openravepy.RaveInitialize(True)
openravepy.misc.InitOpenRAVELogging()
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal)
self.is_setup = True
self.env = openravepy.Environment()
with self.env:
self.env.Load('data/wamtest2.env.xml')
self.robot = self.env.GetRobot('BarrettWAM')
self.manipulator = self.robot.GetManipulator('arm')
self.env.Remove(self.env.GetKinBody('floor'))
self.robot.SetActiveManipulator(self.manipulator)
self.robot.SetActiveDOFs(self.active_dof_indices)
self.planner = self.planner_factory()
for base_cls in self.__class__.__bases__:
if base_cls != BasePlannerTest and hasattr(base_cls, 'setUp'):
base_cls.setUp(self)
def setUp(self):
if not self.is_setup:
openravepy.RaveInitialize(True)
openravepy.misc.InitOpenRAVELogging()
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal)
self.env = openravepy.Environment()
self.robot = self.env.ReadRobotXMLFile(
'robots/barrettwam.robot.xml')
self.env.Add(self.robot)
def setUpClass(cls):
np.set_printoptions(precision=6, suppress=True)
env = orpy.Environment()
if not env.Load('data/lab1.env.xml'):
raise Exception('Could not load scene: data/lab1.env.xml')
robot = env.GetRobot('BarrettWAM')
np.random.seed(123)
q = ru.kinematics.random_joint_values(robot)
robot.SetActiveDOFValues(q)
orpy.RaveSetDebugLevel(orpy.DebugLevel.Fatal)
print('') # dummy line
cls.env = env
cls.robot = robot
def __init__(self, args):
# Initialize values
self.busy = False
self.topic_name = args.topic
rospack = rospkg.RosPack()
path = rospack.get_path('or_octomap_plugin') + '/tests/'
self.file_name = path + args.file_name
self.octomap_resolution = str(args.resolution)
self.octomap_range = args.range
self.octomap_frame = args.frame
self.timeout = args.timeout
self.pause = False
# self.env_name = '' # TODO:
# self.robot_name = '' # TODO:
# self.obstacle_name = '' # TODO:
self.durations = []
# Initialize OpenRAVE environment
self.env = orpy.Environment()
self.env.SetViewer('qtcoin')
orpy.RaveSetDebugLevel(orpy.DebugLevel.Error)
# self.env.Load(self.env_name)
# Initialize ROS subscribers and publisher
self.sub_octomap = rospy.Subscriber('/occupied_cells_vis_array',
MarkerArray,
callback=self.process_octomap,
queue_size=1)
# Initialize octomap plugin
orpy.RaveLoadPlugin("or_octomap") # Load octomap plugin
self.sensor_server = orpy.RaveCreateSensorSystem(
self.env, "or_octomap")
self.sensor_server.SendCommand("Enable") # Enable octomap thread
self.sensor_server.SendCommand(
"ResetTopic " + self.topic_name) # Reset the registered pcl topic
self.sensor_server.SendCommand(
"ResetResolution " +
self.octomap_resolution) # Reset the octomap resolution
self.sensor_server.SendCommand(
"ResetFrameID " + self.octomap_frame) # Reset the octomap frame
# Initialize dynamic reconfigure server
self.server_d = dynamic_reconfigure.server.Server(
OctomapServerConfig, self.cb_reconfig_server, '')
# Reset octomap server parameters
self.sensor_server.SendCommand('TogglePause')
rospy.sleep(0.1)
rospy.set_param('or_octomap/sensor_model_max_range',
self.octomap_range)
self.sensor_server.SendCommand('Reset')
self.time_start = time.time()
def setUp(self):
import openravepy
GeometryInfo = openravepy.KinBody.Link.GeometryInfo
if not self.is_setup:
openravepy.RaveInitialize(True)
openravepy.misc.InitOpenRAVELogging()
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal)
self.is_setup = True
self.env = openravepy.Environment()
with self.env:
self.env.Load('data/wamtest2.env.xml')
self.robot = self.env.GetRobot('BarrettWAM')
self.manipulator = self.robot.GetManipulator('arm')
# Add sphere geometry to the robot for CHOMP.
for link_name, spheres in self.orcdchomp_spheres.iteritems():
link = self.robot.GetLink(link_name)
assert link is not None
geometry_infos = []
for sphere in spheres:
geometry_info = GeometryInfo()
geometry_info._type = openravepy.GeometryType.Sphere
geometry_info._t = numpy.eye(4)
geometry_info._t[0:3, 3] = sphere.position
geometry_info._bModifiable = False
geometry_info._bVisible = False
geometry_info._vGeomData = numpy.array([sphere.radius] * 3)
geometry_infos.append(geometry_info)
link.SetGroupGeometries('spheres', geometry_infos)
# Remove the floor because it can cause spurious collisions.
self.env.Remove(self.env.GetKinBody('floor'))
self.robot.SetActiveManipulator(self.manipulator)
self.robot.SetActiveDOFs(self.active_dof_indices)
self.planner = self.planner_factory()
for base_cls in self.__class__.__bases__:
if base_cls != BasePlannerTest and hasattr(base_cls, 'setUp'):
base_cls.setUp(self)
def chomp_plan(robot, group_name, active_joint_names, active_affine,
target_dof_values, init_trajs):
datadir = 'chomp_data'
n_points = args.n_steps
assert active_affine == 0 or active_affine == 11
use_base = active_affine == 11
saver = openravepy.RobotStateSaver(robot)
target_base_pose = None
if use_base:
with robot:
robot.SetActiveDOFValues(target_dof_values)
target_base_pose = openravepy.poseFromMatrix(robot.GetTransform())
robot.SetActiveDOFs(
robot.GetActiveDOFIndices(),
0) # turn of affine dofs; chomp takes that separately
target_dof_values = target_dof_values[:-3] # strip off the affine part
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Warn)
m_chomp = get_chomp_module(robot.GetEnv())
env_hash = hash_env(robot.GetEnv())
if active_affine != 0:
env_hash += "_aa" + str(active_affine)
# load distance field
j1idxs = [m.GetArmIndices()[0] for m in robot.GetManipulators()]
for link in robot.GetLinks():
for j1idx in j1idxs:
if robot.DoesAffect(j1idx, link.GetIndex()):
link.Enable(False)
try:
aabb_padding = 1.0 if not use_base else 3.0 # base problems should need a distance field over a larger volume
m_chomp.computedistancefield(kinbody=robot,
aabb_padding=aabb_padding,
cache_filename='%s/chomp-sdf-%s.dat' %
(datadir, env_hash))
except Exception, e:
print 'Exception in computedistancefield:', repr(e)
def MoveBaseTo(robot, goal, planner, skip_waypoints=False):
try:
orpy.RaveSetDebugLevel(orpy.DebugLevel.Verbose)
robot.GetEnv().GetKinBody('floor').Enable(False)
xyyaw = np.array(
[goal[0, 3], goal[1, 3],
np.arctan2(goal[1, 0], goal[0, 0])])
current = robot.GetTransform()
currentxyyaw = np.array([
current[0, 3], current[1, 3],
np.arctan2(current[1, 0], current[0, 0])
])
if np.linalg.norm(currentxyyaw - xyyaw) < 0.0001:
print robot.GetName(), ' already at goal. Not moving.'
return
with env:
robot.SetActiveDOFs([], orpy.DOFAffine.X | orpy.DOFAffine.Y
| orpy.DOFAffine.RotationAxis, [0, 0, 1])
traj = planner.MoveActiveJoints(goal=xyyaw,
maxiter=5000,
steplength=0.15,
maxtries=2,
execute=False,
outputtrajobj=True,
jitter=-0.01)
if skip_waypoints:
traj.Remove(1, traj.GetNumWaypoints() - 1)
robot.GetController().SetPath(traj)
while not robot.GetController().IsDone():
time.sleep(0.01)
except Exception, e:
print str(e)
if sim:
print 'Snapping to goal.'
robot.SetTransform(goal)
else:
MoveBack(robot, planner)
# not be available if the user manually set the OPENRAVE_DATA environmental
# variable, e.g. through openrave_catkin.
try:
share_path = subprocess.check_output(['openrave-config',
'--share-dir']).strip()
os.environ['OPENRAVE_DATA'] = os.path.join(share_path, 'data')
except subprocess.CalledProcessError as e:
print(
'error: Failed using "openrave-config" to find the default'
' OPENRAVE_DATA path. Loading assets may fail.',
file=sys.stderr)
# Initialize OpenRAVE.
openravepy.RaveInitialize(True)
openravepy.misc.InitOpenRAVELogging()
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal)
class PlannerTestsMeta(type):
PLANNER_NAMES = [
"BKPIECE1",
"EST",
"KPIECE1",
"LazyRRT",
"LBKPIECE1",
"PRM",
"LazyPRM",
"PRMstar",
"RRT",
"RRTConnect",
"SBL",
bar.update(count)
bar.finish()
return
if __name__ == '__main__':
args = parse_args()
np.set_printoptions(precision=6, suppress=True)
cu.logger.initialize_logging(format_level=logging.INFO)
# Load the OpenRAVE environment
world_xml = 'worlds/airbus_challenge.env.xml'
env = orpy.Environment()
if not env.Load(world_xml):
logger.error('Failed to load: {0}'.format(world_xml))
raise IOError
orpy.RaveSetDebugLevel(orpy.DebugLevel.Fatal)
# Setup robot and manip
robot = env.GetRobot('robot')
manip = robot.SetActiveManipulator('drill')
robot.SetActiveDOFs(manip.GetArmIndices())
qhome = np.deg2rad([0, -20, 130, 0, 70, 0.])
with env:
robot.SetActiveDOFValues(qhome)
# Load IKFast and links stats
iktype = orpy.IkParameterizationType.Transform6D
if not ru.kinematics.load_ikfast(robot, iktype):
logger.error('Failed to load IKFast {0}'.format(iktype.name))
exit()
success = ru.kinematics.load_link_stats(robot, xyzdelta=0.01)
# The robot velocity limits are quite high in the model
robot.SetDOFVelocityLimits([1., 0.7, 0.7, 1., 0.7, 1.57])
while not robot.GetController().IsDone():
time.sleep(0.01)
except Exception, e:
print str(e)
if sim:
print 'Snapping to goal.'
robot.SetTransform(goal)
else:
MoveBack(robot, planner)
#c = raw_input('IPython shell?(y/n)')
#if c == 'y':
# IPython.embed()
#raise e
finally:
robot.GetEnv().GetKinBody('floor').Enable(True)
orpy.RaveSetDebugLevel(orpy.DebugLevel.Info)
def getArmNorm(traj, robot):
traj_start = traj.GetWaypoint(0)
traj_end = traj.GetWaypoint(traj.GetNumWaypoints() - 1) #get last waypoint
first_config = traj.GetConfigurationSpecification().ExtractJointValues(
traj_start, robot, robot.GetActiveDOFIndices())
last_config = traj.GetConfigurationSpecification().ExtractJointValues(
traj_end, robot, robot.GetActiveDOFIndices())
return np.linalg.norm(first_config - last_config, np.inf) #supnorm
def MoveArmTo(robot, goal, planner):
rob.GetLink('link5').Enable(enable)
def EnableGrabbedObjects(env,robot_name,enable):
with env:
rob = env.GetRobot(robot_name)
for o in rob.GetGrabbed():
o.Enable(enable)
<<<<<<< .mine
def MoveBaseTo(robot,goal,planner,skip_waypoints=False):
=======
def MoveBack(robot,planner):
>>>>>>> .r1790
try:
<<<<<<< .mine
orpy.RaveSetDebugLevel(orpy.DebugLevel.Verbose)
=======
EnableGripperLinks(robot.GetEnv(),robot.GetName(),False)
goal = robot.GetTransform()
goal[:2,3] -= 0.20*robot.GetManipulators()[0].GetEndEffectorTransform()[:2,2]
xyyaw = np.array([goal[0,3],goal[1,3],np.arctan2(goal[1,0],goal[0,0])])
with env:
robot.SetActiveDOFs([],orpy.DOFAffine.X|orpy.DOFAffine.Y|orpy.DOFAffine.RotationAxis,[0,0,1])
traj = planner.MoveActiveJoints(goal=xyyaw,maxiter=5000,steplength=0.15,maxtries=2,execute=False,outputtrajobj=True, jitter=-0.01)
robot.GetController().SetPath(traj)
while not robot.GetController().IsDone():
time.sleep(0.01)
except Exception, e:
print 'Exception when moving back.'
print str(e)
c = raw_input('IPython shell?(y/n)')
def MoveBaseTo(robot,goal,planner,skip_waypoints=False):
try:
EnableGripperLinks(robot.GetEnv(),robot.GetName(),False) # HACK TODO XXX
orpy.RaveSetDebugLevel(orpy.DebugLevel.Verbose)
ap.add_argument('--test', action='store_true')
ap.add_argument('--params', default='params/quadrotor.yaml')
return ap.parse_args()
@rave.with_destroy
def run():
args = parse_args()
params = parser.Yaml(file_name=args.params)
env = rave.Environment()
env.SetViewer('qtcoin')
env.Reset()
# load a scene from ProjectRoom environment XML file
env.Load(params.scene)
env.UpdatePublishedBodies()
time.sleep(0.1)
# 1) get the 1st robot that is inside the loaded scene
# 2) assign it to the variable named 'robot'
robot = env.GetRobots()[0]
raw_input("Press enter to exit...")
if __name__ == "__main__":
rave.RaveSetDebugLevel(rave.DebugLevel.Verbose)
run()
action='store_true',
help=
'Enable visualization of tree growth (only applicable for simple robot)'
)
parser.add_argument('-d',
'--debug',
action='store_true',
help='Enable debug logging')
args = parser.parse_args()
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging()
if args.debug:
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug)
env = openravepy.Environment()
env.SetViewer('qtcoin')
env.GetViewer().SetName('Homework 2 Viewer')
# First setup the environment and the robot
visualize = args.visualize
if args.robot == 'herb':
robot = HerbRobot(env, args.manip)
planning_env = HerbEnvironment(robot)
visualize = False
elif args.robot == 'simple':
robot = SimpleRobot(env)
planning_env = SimpleEnvironment(robot)
else:
for i in range(len(marker.points)):
print 'Transform { translation %f %f %f \n ' % (
marker.points[i].x, marker.points[i].y,
marker.points[i].z)
f.write('Transform { translation %f %f %f \n ' %
(marker.points[i].x, marker.points[i].y,
marker.points[i].z))
f.write(
'children [ Shape { appearance Appearance { material Material { diffuseColor %f %f %f } } '
% (marker.colors[i].r, marker.colors[i].g,
marker.colors[i].b) +
'geometry Box { size %f %f %f } } ]\n}\n' %
(marker.scale.x, marker.scale.y, marker.scale.z))
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Error)
if __name__ == "__main__":
file_name = 'crinspect_octomap'
topic_name = '/camera/depth/points'
rospy.init_node('test_octomap', anonymous=False)
env = openravepy.Environment()
# env.Load('robots/denso_with_ftsensor.robot.xml')
env.Load('worlds/lab_demo.env.xml')
robot = env.GetRobot('robot')
robot.SetActiveDOFValues([0, -0.34906585, 2.26892803, 0, 1.22173048, 0])
# env.SetDefaultViewer()
env.SetViewer('qtcoin')
# Initialize octomap
