def setup(sim=False, viewer=None, debug=True):
# load the robot and environment for meal serving
# find the openrave environment file
data_base_path = find_in_workspaces(search_dirs=['share'],
project=project_name,
path='data',
first_match_only=True)
if len(data_base_path) == 0:
raise Exception(
'Unable to find environment path. Did you source devel/setup.bash?'
)
env_path = os.path.join(data_base_path[0], 'environments', 'table.env.xml')
# Initialize logging
if debug:
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Debug)
else:
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging()
prpy.logger.initialize_logging()
# Load the environment and robot
env, robot = adapy.initialize(attach_viewer=viewer,
sim=sim,
env_path=env_path)
# Set the active manipulator on the robot
robot.arm.SetActive()
# Now set everything to the right location in the environment
#if using jaco, assume we have the portable mount, and robot should have a different distance to table
using_jaco = robot.GetName() == 'JACO'
if using_jaco:
robot_pose = numpy.array([[1., 0., 0., 0.409], [0., 1., 0., 0.338],
[0., 0., 1., 0.754], [0., 0., 0., 1.]])
else:
robot_pose = numpy.array([[1., 0., 0., 0.409], [0., 1., 0., 0.338],
[0., 0., 1., 0.795], [0., 0., 0., 1.]])
with env:
robot.SetTransform(robot_pose)
# Set the robot joint configurations
ResetTrial(robot)
load_fork_and_tool(env, robot)
# add boxes for constraint to not hit user
AddConstraintBoxes(env, robot)
return env, robot
def __init__(self, args, endEffName="Mico"):
openravepy.RaveInitialize(True)
openravepy.misc.InitOpenRAVELogging()
self.env, self.robot = adapy.initialize(
sim=args.sim,
attach_viewer=args.viewer,
env_path=args.env_xml
)
# make the robot go at half speed
vel_limits = self.robot.GetDOFVelocityLimits()
self.robot.SetDOFVelocityLimits(vel_limits * 0.5)
self.manip = self.robot.SetActiveManipulator(endEffName)
self.manip_rob = openravepy.interfaces.BaseManipulation(self.robot) # create the interface for basic manipulation programs
self.rot = self.generate_target_rotmat()
self.quat = t3d.quaternions.mat2quat(self.rot)
# even though it's not obvious how we use this, we need to initialize the IKModel on self.robot
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(self.robot,iktype=openravepy.IkParameterization.Type.Transform6D)
#ikmodel.autogenerate()
try:
# ideally this would return 0 instead of erroring if it fails, but for now a try-catch will do the trick
# actually, a try-catch doesn't help. just run autogenerate whenever you change the robot
ikmodel.load()
except:
ikmodel.autogenerate()
self.dist_to_goal_publisher = rospy.Publisher(distance_to_goal_topic, Float64, queue_size=10)
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 rave_env():
if IMPORT_OPENRAVE:
logger.warn("Starting openrave")
orpy.RaveInitialize(load_all_plugins=True)
logger.warn("Starting a new environment")
env = orpy.Environment()
yield env
logger.warn("Destroying a new environment")
env.Destroy()
logger.warn("Destroying Rave runtime")
orpy.RaveDestroy()
else:
yield None
def Initialize_Adapy(args, env_path='/environments/tablewithobjects_assisttest.env.xml'):
""" Initializes robot and environment through adapy, using the specified environment path
@param env_path path to OpenRAVE environment
@return environment, robot
"""
#env_path = '/environments/tablewithobjects_assisttest.env.xml'
adapy_args = {'sim':args.sim,
'attach_viewer':args.viewer,
'env_path':env_path
}
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Debug)
#openravepy.misc.InitOpenRAVELogging();
env, robot = adapy.initialize(**adapy_args)
Init_Robot(robot)
return env,robot
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)
)
parser.add_argument(
'-v',
'--visualize',
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
openrave_data_path = os.getenv('OPENRAVE_DATA', '')
openrave_data_paths = openrave_data_path.split(':')
if ordata_path_thispack not in openrave_data_paths:
if openrave_data_path == '':
os.environ['OPENRAVE_DATA'] = ordata_path_thispack
else:
datastr = str('%s:%s' % (ordata_path_thispack, openrave_data_path))
os.environ['OPENRAVE_DATA'] = datastr
# Set database file to be in this folder only
relative_ordatabase = '/database'
ordatabase_path_thispack = curr_path + relative_ordatabase
os.environ['OPENRAVE_DATABASE'] = ordatabase_path_thispack
samples = 10
# Get rid of warnings
openravepy.RaveInitialize(True, openravepy.DebugLevel.Fatal)
openravepy.misc.InitOpenRAVELogging()
class RoboHandler:
def __init__(self):
self.openrave_init()
self.problem_init()
print "Start Ordering Grasps"
# Order grasps based on your own scoring metric
#self.order_grasps()
print "Finished Ordering Grasps"
# Order grasps with noise
print "Start Ordering Noisy Grasps"
self.order_grasps_noisy()
print "Finished Ordering Noisy Grasps"
type=str,
help='environment XML file; defaults to an empty environment')
parser.add_argument('-b',
'--segway-sim',
action='store_true',
help='simulate base')
parser.add_argument('-p',
'--perception-sim',
action='store_true',
help='simulate perception')
parser.add_argument('--debug',
action='store_true',
help='enable debug logging')
args = parser.parse_args()
openravepy.RaveInitialize(True)
openravepy.misc.InitOpenRAVELogging()
if args.debug:
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug)
herbpy_args = {
'sim': args.sim,
'attach_viewer': args.viewer,
'robot_xml': args.robot_xml,
'env_path': args.env_xml,
'segway_sim': args.segway_sim,
'perception_sim': args.perception_sim
}
if args.sim and not args.segway_sim:
herbpy_args['segway_sim'] = args.sim
def main():
"""Execute demo for grasping glass."""
parser = argparse.ArgumentParser()
parser.add_argument('--viewer',
'-v',
type=str,
default='interactivemarker',
help='The viewer to attach (none for no viewer)')
parser.add_argument('--monitor',
action='store_true',
help='Display a UI to monitor progress of the planner')
parser.add_argument('--planner',
type=str,
choices=['dfs', 'restart'],
default='restart',
help='The planner to use')
parser.add_argument('--robot',
type=str,
default='herb',
help='Robot to run the task on')
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging()
args = parser.parse_args()
env, robot = herbpy.initialize()
# Get the desired manipulator
manipulator = robot.GetManipulator('right')
if args.viewer != 'none':
env.SetViewer(args.viewer)
monitor = None
# Create a monitor
if args.monitor:
monitor = magi.monitor.ActionMonitor()
def signal_handler(signum, frame):
"""Signal handler to gracefully kill the monitor."""
monitor.stop()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)
# Create a planner
if args.planner == 'restart':
planner = RestartPlanner(monitor=monitor)
elif args.planner == 'dfs':
planner = DepthFirstPlanner(monitor=monitor, use_frustration=True)
if monitor is not None:
monitor.reset()
# Detect objects
table, glass = detect_objects(robot)
try:
# Create the task.
action = grasp_glass_action_graph(manipulator, glass, table)
# Plan the task
with env:
solution = planner.plan_action(env, action)
# Execute the task
execute_pipeline(env, solution, simulate=True, monitor=monitor)
except ActionError as err:
LOGGER.info('Failed to complete planning for task: %s', str(err))
raise
except ExecutionError as err:
LOGGER.info('Failed to execute task: %s', str(err))
raise
IPython.embed()
if monitor:
monitor.stop()
'-v',
"validate_result",
is_flag=True,
help="Validate DH parameters before returning")
@click.option("--viewer",
"enable_viewer",
is_flag=True,
help="Enable OpenRAVE viewer")
def cli(robot, base_link, ee_link, modified_dh, validate_result, enable_viewer,
env):
""" Compute DH parameters of the ROBOT kinematic chain defined by BASE_LINK and EE_LINK.
Example usage: `python openrave.py robots/pr2-beta-static.zae base_link \\ r_gripper_palm_link`
"""
if enable_viewer:
env.SetViewer("qtcoin")
dh = get_dh_table(robot, base_link, ee_link, modified_dh, validate_result)
click.echo(dh)
if enable_viewer:
input("Press any key to quit...")
if __name__ == '__main__':
try:
rave.RaveInitialize(load_all_plugins=True, level=rave.DebugLevel.Error)
cli()
finally:
rave.RaveDestroy()
def initialize(robot_xml=None, env_path=None, attach_viewer=False,
sim=True, **kw_args):
import prpy, os
prpy.logger.initialize_logging()
# Hide TrajOpt logging.
os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN')
# Load plugins.
if prpy.dependency_manager.is_catkin():
prpy.dependency_manager.export()
else:
prpy.dependency_manager.export(PACKAGE)
openravepy.RaveInitialize(True)
# Create the environment.
env = openravepy.Environment()
if env_path is not None:
if not env.Load(env_path):
raise Exception('Unable to load environment frompath %s' % env_path)
# Load the robot
model_name = None
share_directories = None
if robot_xml is None:
if prpy.dependency_manager.is_catkin():
from catkin.find_in_workspaces import find_in_workspaces
share_directories = find_in_workspaces(search_dirs=['share'],
project=PACKAGE)
if not share_directories:
logger.error('Unable to find share directory in catkin workspace.')
raise ValueError('Unable to find share directory in catkin workspace.')
import os.path
model_name = 'barrettsegway.robot.xml'
for share_directory in share_directories:
xml_path = os.path.join(share_directory, 'robots', model_name)
if os.path.exists(xml_path):
robot_xml = xml_path
if robot_xml is None:
err_str = 'Unable to find robot model with name: %s in directories %s.' % (model_name, share_directories)
logger.error(err_str)
raise ValueError(err_str)
# Load the robot model
robot = env.ReadRobotXMLFile(robot_xml)
env.Add(robot)
# Default arguments.
keys = [ 'arm_sim',
'hand_sim',
'ft_sim',
'segway_sim'
]
for key in keys:
if key not in kw_args:
kw_args[key] = sim
from wamrobot import WAMRobot
prpy.bind_subclass(robot, WAMRobot, **kw_args)
if sim:
dof_indices, dof_values \
= robot.configurations.get_configuration('zero')
robot.SetDOFValues(dof_values, dof_indices)
# Start by attempting to load or_rviz.
if attach_viewer == True:
attach_viewer = 'rviz'
env.SetViewer(attach_viewer)
# Fall back on qtcoin if loading or_rviz failed
if env.GetViewer() is None:
logger.warning(
'Loading the RViz viewer failed. Do you have or_interactive'
' marker installed? Falling back on qtcoin.')
attach_viewer = 'qtcoin'
if attach_viewer and env.GetViewer() is None:
env.SetViewer(attach_viewer)
if env.GetViewer() is None:
raise Exception('Failed creating viewer of type "{0:s}".'.format(
attach_viewer))
# Remove the ROS logging handler again.
# It might have been added when we loaded or_rviz.
prpy.logger.remove_ros_logger()
return env, robot
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#based on HW3 for RBE 595/CS 525 Motion Planning
import time
import openravepy
openravepy.RaveInitialize()
openravepy.RaveLoadPlugin('build/rrt-pluguin')
import multiprocessing, math, time
if not __openravepy_build_doc__:
from openravepy import *
from numpy import *
def waitrobot(robot):
"""busy wait for robot completion"""
while not robot.GetController().IsDone():
time.sleep(0.01)
def tuckarms(env, robot):
with env:
jointnames = [
'l_shoulder_lift_joint', 'l_elbow_flex_joint',
'l_wrist_flex_joint', 'r_shoulder_lift_joint',
'r_elbow_flex_joint', 'r_wrist_flex_joint'
]
robot.SetActiveDOFs(
[robot.GetJoint(name).GetDOFIndex() for name in jointnames])
robot.SetActiveDOFValues([
def setup(sim=False, viewer=None, debug=False):
data_base_path = find_in_workspaces(search_dirs=['share'],
project=project_name,
path='data',
first_match_only=True)
if len(data_base_path) == 0:
raise Exception(
'Unable to find environment path. Did you source devel/setup.bash?'
)
env_path = os.path.join(data_base_path[0], 'environments', 'table.env.xml')
# Initialize logging
if debug:
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Debug)
else:
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
openravepy.misc.InitOpenRAVELogging()
# Load the environment
env = openravepy.Environment()
if not env.Load(env_path):
raise IOError('Unable to load environment "{:s}".'.format(env_path))
with env:
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
ada_name = or_urdf.SendCommand('LoadURI {:s} {:s}'.format(
URDF_PATH, SRDF_PATH))
env.SetViewer('InteractiveMarker')
robot = env.GetRobot(ada_name)
#TODO get this from a rosparam
right_handed = True
# Set the active manipulator on the robot
robot.arm = robot.GetManipulator('Mico')
# robot.arm.SetActive()
# Now set everything to the right location in the environment
#if using jaco, assume we have the portable mount, and robot should have a different distance to table
using_jaco = robot.GetName() == 'JACO'
if using_jaco:
robot_pose = numpy.array([[1., 0., 0., 0.409], [0., 1., 0., 0.338],
[0., 0., 1., 0.754], [0., 0., 0., 1.]])
else:
robot_pose = numpy.array([[1., 0., 0., 0.409], [0., 1., 0., 0.338],
[0., 0., 1., 0.795], [0., 0., 0., 1.]])
with env:
robot.SetTransform(robot_pose)
#if sim is True:
# startConfig = numpy.array([ 3.33066907e-16, 2.22044605e-16, 1.66608370e+00,
# -1.65549603e+00, -1.94424475e-01, 1.06742772e+00,
# -1.65409614e+00, 1.30780704e+00])
logger.info('Setting Initial Robot Configuration to Serving')
# robot.SetDOFValues(startConfig)
logger.info('Initial Config Set')
# Load the fork into the robot's hand
tool = env.ReadKinBodyURI('objects/kinova_tool.kinbody.xml')
env.Add(tool)
# Fork in end-effector
#ee_in_world = robot.GetLink('j2n6a300_link_6').GetTransform()
tool_in_ee = numpy.array([[-1., 0., 0., 0.], [0., 1., 0., -0.002],
[0., 0., -1., -0.118], [0., 0., 0., 1.]])
ee_in_world = robot.arm.GetEndEffectorTransform()
if right_handed:
y_trans_tool = 0.004
else:
y_trans_tool = -0.004
tool_in_ee = numpy.array([[1., 0., 0., 0.], [0., 1., 0., y_trans_tool],
[0., 0., 1., -0.042], [0., 0., 0., 1.]])
rotate_tool_in_ee = rodrigues([0., 0., 0.])
rotate_tool_in_ee = rodrigues([0., 0., numpy.pi / 32.])
#rotate_tool_in_ee = rodrigues([0., 0., -np.pi/32.])
tool_in_ee[0:3, 0:3] = numpy.dot(rotate_tool_in_ee, tool_in_ee[0:3, 0:3])
tool_in_world = numpy.dot(ee_in_world, tool_in_ee)
tool.SetTransform(tool_in_world)
fork = env.ReadKinBodyURI('objects/fork.kinbody.xml')
env.Add(fork)
fork_in_hole = numpy.array([[1., 0., 0., 0.], [0., 1., 0., 0.],
[0., 0., 1., -0.03], [0., 0., 0., 1.]])
hole_in_tool = numpy.array([[0., 0., 1., 0.], [0., 1., 0., -0.0225],
[-1., 0., 0., 0.0408], [0., 0., 0., 1.]])
fork_in_tool = numpy.dot(hole_in_tool, fork_in_hole)
fork_in_ee = numpy.dot(tool_in_ee, fork_in_tool)
fork_in_world = numpy.dot(ee_in_world, fork_in_ee)
fork.SetTransform(fork_in_world)
logger.info('creating fork and tool boxes')
# fork_box = make_collision_box_body(fork, add_to_pos=np.array([0.0, 0.0, 0.05]), add_to_extents=np.array([0.02, 0.02, 0.1]))
# tool_box = make_collision_box_body(tool, add_to_pos=np.array([0.0, 0.0, 0.04]), add_to_extents=np.array([0.055, 0.055, 0.055]))
# logger.info('fork and tool boxes created')
#find all finger links
# finger_link_inds = []
# grab_link = None
# for ind,link in enumerate(robot.GetLinks()):
# if 'inger' in link.GetName():
# finger_link_inds.append(ind)
# if 'end_effector' in link.GetName():
# grab_link = link
#
# logger.info('Grabbing tool and fork')
robot.Grab(tool)
robot.Grab(fork)
# robot.Grab(tool_box, grablink=grab_link, linkstoignore=finger_link_inds)
# robot.Grab(fork_box, grablink=grab_link, linkstoignore=finger_link_inds)
return robot, env
