def initialize(model_filename='jaco', envXML=None):
'''
Load and configure the JACO robot. If envXML is not None, loads environment.
Returns robot and environment.
'''
env = openravepy.Environment()
if envXML is not None:
env.LoadURI(envXML)
env.SetViewer('qtcoin')
# Assumes the robot files are located in the data folder of the
# kinova_description package in the catkin workspace.
urdf_uri = 'package://iact_control/src/data/'+model_filename+'.urdf'
srdf_uri = 'package://iact_control/src/data/'+model_filename+'.srdf'
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
robot_name = or_urdf.SendCommand('load {:s} {:s}'.format(urdf_uri, srdf_uri))
robot = env.GetRobot(robot_name)
bind_subclass(robot, ArchieRobot)
robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]))
robot.SetDOFValues(robot_starting_dofs)
viewer = env.GetViewer()
viewer.SetSize(500,500)
cam_params = np.array([[-0.99885711, -0.01248719, -0.0461361 , -0.18887213],
[ 0.02495645, 0.68697757, -0.72624996, 2.04733515],
[ 0.04076329, -0.72657133, -0.68588079, 1.67818344],
[ 0. , 0. , 0. , 1. ]])
viewer.SetCamera(cam_params)
#viewer.SetBkgndColor([0.8,0.8,0.8])
return env, robot
def main():
global m
global e
e = r.Environment()
m = r.RaveCreateModule(e, 'orchomp')
e.SetViewer('qtcoin')
e.Load("intelkitchen_robotized_herb2_nosensors.env.xml")
robot = e.GetRobot("Herb2")
# set the manipulator to leftarm
#ikmodel = databases.inversekinematics.InverseKinematicsModel(
# robot,iktype=IkParameterization.Type.Transform6D)
#if not ikmodel.load():
# ikmodel.autogenerate()
Tz = r.matrixFromAxisAngle([0, 0, numpy.pi / 2])
Tz[0, 3] = 0.4
Tz[1, 3] = 1.6
Tz[2, 3] = -0.01
print Tz
with e:
for body in e.GetBodies():
body.SetTransform(numpy.dot(Tz, body.GetTransform()))
readFileCommands()
def main():
global e
global m
global robot
global knownstates
e = r.Environment()
m = r.RaveCreateModule(e, 'orcdchomp')
e.SetViewer('qtcoin')
#e.Load( 'lab1.env.xml' );
e.Load('herb2_padded_nosensors.robot.xml')
robot = e.GetRobot('Herb2')
knownstates = readKnownStates("data/knownstates.data")
printKnownStates(knownstates)
return
#robot.SetActiveManipulator( 0 )
filename = "randomstates2.data"
#filename = "randomManipulatorStates_0.data"
userGetRandomStates(filename)
return
def __init__(self, env, robotname):
"""
Create a ElasticStrips Problem Instance
:param env: rave.Environment
:param robotname: str
:return: ElasticStrips
"""
print('ElasticStrips.py initialize')
self.module = rave.RaveCreateModule(env, 'ElasticStrips')
env.AddModule(self.module, '')
self.env = env
self.robotname = robotname
# self.problem = rave.RaveCreateProblem(env, 'CBiRRT')
# env.LoadProblem(self.problem, robotname)
# Build a mapping for manipulators to manipulator indices
self.manip_indices = {}
for index, manip in enumerate(
env.GetRobot(robotname).GetManipulators()):
self.manip_indices[manip] = index
self.manip_indices[manip.GetName()] = index
def setUp(self):
self.env = openravepy.Environment()
urdf_module = openravepy.RaveCreateModule(self.env, 'urdf')
with self.env:
arg = 'load {:s}'.format(urdf_path)
bh_name = urdf_module.SendCommand(arg)
self.bh_body = self.env.GetKinBody(bh_name)
def __init__(self, env, urdf_path, srdf_path):
self.env = env
# Load robot
module = rave.RaveCreateModule(self.env, 'urdf')
robot_name = module.SendCommand('load {} {}'.format(
urdf_path, srdf_path))
self.robot = self.env.GetRobot(robot_name)
self.manip = AttributePassthrough(self.robot.GetManipulator,
self.robot.GetManipulators)
def initialize(env_path=None, attach_viewer=False, **kw_args):
from adarobot import ADARobot
from util import AdaPyException, find_adapy_resource
prpy.logger.initialize_logging()
# Create the environment.
env = openravepy.Environment()
if env_path is not None:
if not env.Load(env_path):
raise IOError(
'Unable to load environment "{:s}".'.format(env_path))
# Use or_urdf to load ADA from URDF and SRDF.
with env:
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
ada_name = or_urdf.SendCommand('load {:s} {:s}'.format(
URDF_PATH, SRDF_PATH))
robot = env.GetRobot(ada_name)
if robot is None:
raise AdaPyException('Failed loading ADA with or_urdf.')
# Bind AdaPy-specific functionality on the robot.
prpy.bind_subclass(robot, ADARobot, **kw_args)
# 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 rviz failed. Falling back on qt_coin.')
attach_viewer = 'qtcoin'
if attach_viewer and env.GetViewer() is None:
#Using qtcoin as or_rviz gives seg fault
attach_viewer = 'qtcoin'
env.SetViewer(attach_viewer)
if env.GetViewer() is None:
raise AdaPyException(
'Failed creating viewer of type "{: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()
import adapy.action # register actions
import adapy.tsr # register TSR libraries
robot.actions = prpy.action.ActionLibrary()
return env, robot
def initialize(model_filename='jaco_dynamics', visualize=True, envXML=None):
'''
Load and configure the JACO robot. If envXML is not None, loads environment.
Returns robot and environment.
-----
NOTE:
IF YOU JUST WANT TO DO COMPUTATIONS THROUGH OPENRAVE
AND WANT MULTPILE INSTANCES TO OPEN, THEN HAVE TO TURN OFF
QTCOIN AND ALL VIEWER FUNCTIONALITY OR IT WILL CRASH.
'''
env = openravepy.Environment()
if envXML is not None:
env.LoadURI(envXML)
if visualize:
env.SetViewer('qtcoin')
here = os.path.dirname(os.path.realpath(__file__))
urdf_uri = '../resources/' + model_filename + '.urdf'
srdf_uri = '../resources/' + model_filename + '.srdf'
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
if or_urdf is None:
logger.error('Unable to load or_urdf module. Do you have or_urdf'
' built and installed in one of your Catkin workspaces?')
raise ValueError('Unable to load or_urdf plugin.')
robot_name = or_urdf.SendCommand('load {:s} {:s}'.format(
urdf_uri, srdf_uri))
if robot_name is None:
raise ValueError('Failed loading robot model using or_urdf.')
robot = env.GetRobot(robot_name)
if robot is None:
raise ValueError(
'Unable to find robot with name "{:s}".'.format(robot_name))
bind_subclass(robot, ArchieRobot)
robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]))
robot.SetDOFValues(robot_starting_dofs)
if visualize:
viewer = env.GetViewer()
viewer.SetSize(700, 500)
cam_params = np.array(
[[-0.99885711, -0.01248719, -0.0461361, -0.18887213],
[0.02495645, 0.68697757, -0.72624996, 2.04733515],
[0.04076329, -0.72657133, -0.68588079, 1.67818344],
[0., 0., 0., 1.]])
viewer.SetCamera(cam_params)
return env, robot
def start_recording(self,
fname='output.mpg',
codec=13,
framerate=24,
width=640,
height=480):
vname = self.env.GetViewer().GetName()
cmd = 'Start %d %d %d codec %d timing simtime filename %s\nviewer %s'
cmd = cmd % (width, height, framerate, codec, fname, vname)
recorder = openravepy.RaveCreateModule(self.env, 'viewerrecorder')
self.env.AddModule(recorder, '')
recorder.SendCommand(cmd)
self.recorder = recorder
def get_chomp_module(env):
if args.planner == "chomp":
from orcdchomp import orcdchomp
chomp_module_path = "/home/jonathan/build/chomp/liborcdchomp.so"
elif args.planner == "chomp2":
from orcdchomp2 import orcdchomp
chomp_module_path = "/home/jonathan/code/orcdchomp2/lib/liborcdchomp2.so"
else:
assert False
openravepy.RaveLoadPlugin(chomp_module_path)
m_chomp = openravepy.RaveCreateModule(env, 'orcdchomp')
env.Add(m_chomp, True, 'blah_load_string')
orcdchomp.bind(m_chomp)
return m_chomp
def initialize():
'''Load and configure the Archie robot. Returns robot and environment.'''
env = openravepy.Environment()
env.SetViewer('qtcoin')
# Assumes the robot files are located in the urdf folder of the
# kinova_description package in the catkin workspace.
#env.Load('lab.xml')
urdf_uri = 'package://interactpy/jaco_dynamics.urdf'
srdf_uri = 'package://interactpy/jaco_dynamics.srdf'
objects_path = find_in_workspaces(project='interactpy',
path='envdata',
first_match_only=True)[0]
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
robot_name = or_urdf.SendCommand('load {:s} {:s}'.format(
urdf_uri, srdf_uri))
robot = env.GetRobot(robot_name)
bind_subclass(robot, ArchieRobot)
# Put robot in natural starting config.
# Load environment
#env.Load('{:s}/table.xml'.format(objects_path))
env.Load('{:s}/mug1.kinbody.xml'.format(objects_path))
# table = env.GetKinBody('table')J
# table.SetTransform(np.array([[1.00000000e+00, -2.79931237e-36, 1.41282351e-14, 1.50902510e-01],
# [-2.07944729e-28, 1.00000000e+00, 1.47183799e-14, -1.47385532e-02],
# [-1.41282351e-14, -1.47183799e-14, 1.00000000e+00, -1.00134850e-01],
# [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]))
mug = env.GetKinBody('mug')
# mug.SetTransform(np.array([[1.00000000e+00, 0.00000000e+00, 0.00000000e+00, 6.57676935e-01],
# [0.00000000e+00, 0.00000000e+00, -1.00000000e+00, -3.07691471e-06],
# [0.00000000e+00, 1.00000000e+00, 0.00000000e+00, 5.46909690e-01],
# [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]))
#mug.SetTransform(np.array([[1.00000000e+00, 0.00000000e+00, 0.00000000e+00, -3.00000000e-01],
# [0.00000000e+00, 0.00000000e+00, -1.00000000e+00, 1.00000000e+00],
# [0.00000000e+00, 1.00000000e+00, 0.00000000e+00, -1.00000000e-01],
# [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]))
# mug.SetTransform(np.array([[1.00000000e+00, 0.00000000e+00, 0.00000000e+00, -0.10000000],
# [0.00000000e+00, 0.00000000e+00, -1.00000000e+00, 0.20000000e+00],
# [0.00000000e+00, 1.00000000e+00, 0.00000000e+00, -1.00000000e-01],
# [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]))
mug.SetTransform([[1, 0, 0, -0.4], [0, 0, -1, 0.3], [0, 1, 0, -0.1]])
#mug.SetTransform([[1,0,0,-0.6],[0,0,-1,-0.6],[0,1,0,.7]])
robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]))
robot.SetDOFValues(robot_starting_dofs)
return env, robot, mug
def initialize(self, robot):
# construct a new module/planner if necessary
if self.module is None:
self.module = openravepy.RaveCreateModule(self.env, 'Family')
if self.module is None:
raise prpy.planning.base.UnsupportedPlanningError(
'Unable to create Family module.')
if self.planner is None:
self.planner = openravepy.RaveCreatePlanner(
self.env, 'FamilyPlanner')
if self.planner is None:
raise prpy.planning.base.UnsupportedPlanningError(
'Unable to create FamilyPlanner planner.')
def __init__(self):
super(CHOMPPlanner, self).__init__()
try:
from orchomp import orchomp
self.module = openravepy.RaveCreateModule(self.env, 'orchomp')
except ImportError:
raise UnsupportedPlanningError('Unable to import orchomp.')
except openravepy.openrave_exception as e:
raise UnsupportedPlanningError(
'Unable to create orchomp module: %s' % e)
if self.module is None:
raise UnsupportedPlanningError('Failed loading module.')
self.initialized = False
orchomp.bind(self.module)
def initialize():
'''Load and configure the Archie robot. Returns robot and environment.'''
env = openravepy.Environment()
env.SetViewer('qtcoin')
# Assumes the robot files are located in the urdf folder of the
# kinova_description package in the catkin workspace.
urdf_uri = 'package://interactpy/jaco.urdf'
srdf_uri = 'package://interactpy/jaco.srdf'
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
robot_name = or_urdf.SendCommand(
'load {:s} {:s}'.format(urdf_uri, srdf_uri))
robot = env.GetRobot(robot_name)
bind_subclass(robot, ArchieRobot)
# Put robot in natural starting config.
# TODO(allanzhou): Decide if this is really necessary.
robot.SetDOFValues([0, 3, 0, 2, 0, 4, 0, 0, 0, 0])
return env, robot
def runtrial(Tz, useArm=False, useTwoArms=False, view=False):
global e
global m
e = r.Environment()
m = r.RaveCreateModule(e, 'orchomp')
if view:
e.SetViewer('qtcoin')
#e.Load( 'lab1.env.xml')
e.Load("intelkitchen_robotized_herb2_nosensors.env.xml")
robot = e.GetRobot("Herb2")
"""
with e:
for body in e.GetBodies():
body.SetTransform(numpy.dot(Tz,body.GetTransform()))
"""
e.Load("herb_base.kinbody.xml")
herb_body = filter(lambda x: x.GetName() == "herb_base", e.GetBodies())[0]
with e:
robot.SetTransform(numpy.dot(Tz, robot.GetTransform()))
herb_body.SetTransform(robot.GetTransform())
#e.Load( "herb2_padded_nosensors.robot.xml" )
if useTwoArms:
dof1 = list(robot.GetManipulators()[0].GetArmIndices())
dof1 += list(robot.GetManipulators()[1].GetArmIndices())
robot.SetActiveDOFs(dof1)
print dof1
if useArm and not useTwoArms:
robot.SetActiveDOFs(robot.GetManipulators()[0].GetArmIndices())
m.SendCommand("computedistancefield getall robot Herb2")
e.Remove(herb_body)
m.SendCommand("benchmark n 10000")
e.Destroy()
m.Destroy()
def deserialize_environment(data, env=None, purge=False, reuse_bodies=None):
import openravepy
if env is None:
env = openravepy.Environment()
if reuse_bodies is None:
reuse_bodies = []
reuse_bodies_dict = { body.GetName(): body for body in reuse_bodies }
reuse_bodies_set = set(reuse_bodies)
# Release anything that's grabbed.
for body in reuse_bodies:
body.ReleaseAllGrabbed()
# Remove any extra bodies from the environment.
for body in env.GetBodies():
if body not in reuse_bodies_set:
deserialization_logger.debug('Purging body "%s".', body.GetName())
env.Remove(body)
# Create a or_ordf module on demand
urdf_module_getter = UnitaryMemoizer(lambda: openravepy.RaveCreateModule(env,'urdf'))
# Deserialize the kinematic structure.
deserialized_bodies = []
for body_data in data['bodies']:
body = reuse_bodies_dict.get(body_data['name'], None)
if body is None:
body = deserialize_kinbody(env, body_data, state=False,
urdf_module_getter=urdf_module_getter)
deserialization_logger.debug('Deserialized body "%s".', body.GetName())
deserialized_bodies.append((body, body_data))
# Restore state. We do this in a second pass to insure that any bodies that
# are grabbed already exist.
for body, body_data in deserialized_bodies:
deserialize_kinbody_state(body, body_data['kinbody_state'])
if body.IsRobot():
deserialize_robot_state(body, body_data['robot_state'])
return env
def setupEnv(self, env):
from types import MethodType
self.env = env
try:
from orcdchomp import orcdchomp
module = openravepy.RaveCreateModule(self.env, 'orcdchomp')
except ImportError:
raise UnsupportedPlanningError(
'Unable to import "orcdchomp". Is or_cdchomp installed?')
if module is None:
raise UnsupportedPlanningError(
'Failed loading "orcdchomp" module')
if module is None:
raise UnsupportedPlanningError(
'Failed loading "orcdchomp" module. Is or_cdchomp installed?')
# This is a hack to prevent leaking memory.
class CHOMPBindings(object):
pass
self.module = CHOMPBindings()
self.module.module = module
self.module.viewspheres =\
MethodType(orcdchomp.viewspheres, module)
self.module.computedistancefield =\
MethodType(orcdchomp.computedistancefield, module)
self.module.addfield_fromobsarray =\
MethodType(orcdchomp.addfield_fromobsarray, module)
self.module.removefield = MethodType(orcdchomp.removefield, module)
self.module.create = MethodType(orcdchomp.create, module)
self.module.iterate = MethodType(orcdchomp.iterate, module)
self.module.gettraj = MethodType(orcdchomp.gettraj, module)
self.module.destroy = MethodType(orcdchomp.destroy, module)
self.module.runchomp = MethodType(orcdchomp.runchomp, module)
self.module.GetEnv = self.module.module.GetEnv
# Create a DistanceFieldManager to track which distance fields are
# currently loaded.
self.distance_fields = DistanceFieldManager(
self.module, require_cache=self.require_cache)
def main():
global m
global e
e = r.Environment()
m = r.RaveCreateModule(e, 'orchomp')
e.SetViewer('qtcoin')
v = e.GetViewer()
start = [
5.65, -1.76, -0.26, 1.96, -1.15, 0.87, -1.43, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.64, -1.76, 0.26,
1.96, 1.16, 0.87, 1.43, -1.66533454e-16, -1.66533454e-16,
-1.38777878e-16, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00
]
Tz = numpy.array([[-0.81596105, 0.24022406, -0.52583264, 1.39736962],
[0.57709228, 0.28460075, -0.76548476, 1.4022727],
[-0.03423549, -0.9280597, -0.37085458, 1.57485259],
[0., 0., 0., 1.]])
v.SetCamera(Tz)
e.Load("herb2_padded_nosensors.robot.xml")
robot = e.GetRobot("Herb2")
with e:
robot.SetName("original")
e.Load("herb2_padded_nosensors.robot.xml")
robot2 = e.GetRobot("Herb2")
with e:
robot.SetDOFValues(start)
robot2.SetDOFValues(start)
"""
for link in robot.GetLinks():
for geom in link.GetGeometries():
geom.SetTransparency( 0.3 )
"""
readFileCommands()
def __init__(self):
self.env = openravepy.Environment()
rospack = rospkg.RosPack()
package_path = rospack.get_path('jaco_manipulation')
jaco_urdf_path = package_path + '/config/jaco.urdf'
jaco_srdf_path = package_path + '/config/jaco.srdf'
rospy.loginfo("Loading Jaco URDF from {} and SRDF from {}...".format(
jaco_urdf_path, jaco_srdf_path))
self.urdf_module = openravepy.RaveCreateModule(self.env, 'urdf')
name = self.urdf_module.SendCommand("load {} {}".format(
jaco_urdf_path, jaco_srdf_path))
self.jaco = self.env.GetRobot(name)
# self.finger_joint_values = [0.0, 0.0, 0.0]
self.finger_joint_values = [1.0, 1.0, 1.0]
self.joint_names = ['j2s7s300_joint_{}'.format(i) for i in range(1, 8)]
self.trajopt_num_waypoints = 30
self.dt = 0.2 #time between waypoints to be added in post processing
self.cost_functions = []
def initialize(model_filename='jaco', envXML=None, viewer=True):
'''
Load and configure the JACO robot. If envXML is not None, loads environment.
Returns robot and environment.
-----
NOTE:
IF YOU JUST WANT TO DO COMPUTATIONS THROUGH OPENRAVE
AND WANT MULTPILE INSTANCES TO OPEN, THEN HAVE TO TURN OFF
QTCOIN AND ALL VIEWER FUNCTIONALITY OR IT WILL CRASH.
'''
env = openravepy.Environment()
if envXML is not None:
env.LoadURI(envXML)
# Assumes the robot files are located in the data folder of the
# kinova_description package in the catkin workspace.
urdf_uri = 'package://iact_control/src/data/' + model_filename + '.urdf'
srdf_uri = 'package://iact_control/src/data/' + model_filename + '.srdf'
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
robot_name = or_urdf.SendCommand('load {:s} {:s}'.format(
urdf_uri, srdf_uri))
robot = env.GetRobot(robot_name)
bind_subclass(robot, ArchieRobot)
robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]))
robot.SetDOFValues(robot_starting_dofs)
if viewer:
env.SetViewer('qtcoin')
viewer = env.GetViewer()
viewer.SetSize(700, 500)
cam_params = np.array(
[[-0.99885711, -0.01248719, -0.0461361, -0.18887213],
[0.02495645, 0.68697757, -0.72624996, 2.04733515],
[0.04076329, -0.72657133, -0.68588079, 1.67818344],
[0., 0., 0., 1.]])
viewer.SetCamera(cam_params)
return env, robot
def main():
global m
global e
e = r.Environment()
m = r.RaveCreateModule(e, 'orchomp')
e.SetViewer('qtcoin')
e.Load("intelkitchen_robotized_herb2_nosensors_2.env.xml")
robot = e.GetRobot("Herb2")
start = [
3.68000000e+00, -1.90000000e+00, 0.00000000e+00, 2.20000000e+00,
-2.22044605e-16, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.60000000e+00,
-1.90000000e+00, 0.00000000e+00, 2.20000000e+00, 0.00000000e+00,
-1.11022302e-16, 0.00000000e+00, -1.66533454e-16, -1.66533454e-16,
-1.38777878e-16, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00
]
robot.SetDOFValues(start)
readFileCommands()
def __init__(self, path=None, xml=None, use_urdf=False):
assert path is not None or xml is not None
env = get_openrave_env()
if use_urdf:
urdf_path = path[0]
srdf_path = path[1]
module = openravepy.RaveCreateModule(env, 'urdf')
name = module.SendCommand('load {} {}'.format(
urdf_path, srdf_path))
else:
name = basename(splitext(path)[0])
if xml is None:
xml = Robot.__default_xml % (path, name)
env.LoadData(xml)
rave = env.GetRobot(name)
nb_dofs = rave.GetDOF()
q_min, q_max = rave.GetDOFLimits()
rave.SetDOFVelocityLimits([1000.] * nb_dofs)
self.has_free_flyer = False
self.ik = None
self.is_visible = True
self.mass = sum([link.GetMass() for link in rave.GetLinks()])
self.nb_dofs = nb_dofs
self.q_max = q_max
self.q_max.flags.writeable = False
self.q_min = q_min
self.q_min.flags.writeable = False
self.qd_lim = rave.GetDOFVelocityLimits()
self.rave = rave
self.stance = None
self.tau_max = None # set by hand in child robot class
self.transparency = 0. # initially opaque
#
self.ik = IKSolver(self)
def environment_setup(self):
self.env = openravepy.Environment()
InitOpenRAVELogging()
module = openravepy.RaveCreateModule(self.env, 'urdf')
rospack = rospkg.RosPack()
with self.env:
# name = module.SendCommand('load /home/peppe/python_test/sawyer.urdf /home/peppe/python_test/sawyer_base.srdf')
name = module.SendCommand('loadURI ' +
rospack.get_path('fredsmp_utils') +
'/robots/sawyer/sawyer.urdf' + ' ' +
rospack.get_path('fredsmp_utils') +
'/robots/sawyer/sawyer.srdf')
self.robot = self.env.GetRobot(name)
time.sleep(0.5)
self.ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
self.robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not self.ikmodel.load():
self.ikmodel.autogenerate()
manip = self.robot.GetActiveManipulator()
self.robot.SetActiveDOFs(manip.GetArmIndices())
#!/usr/bin/env python
import os
import openravepy
import numpy as np
import time
from openravepy.misc import InitOpenRAVELogging
InitOpenRAVELogging()
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Error)
env = openravepy.Environment()
plugin = openravepy.RaveCreateModule(env, "urdf")
env.SetViewer('qtcoin')
with env:
name = plugin.SendCommand('load real_sawyer_urdf_rounded.xml real_sawyer.srdf')
robot = env.GetRobot(name)
robot.SetActiveManipulator('arm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D
)
if not ikmodel.load():
ikmodel.autogenerate()
m = robot.GetActiveManipulator()
robot.SetActiveDOFs(m.GetArmIndices())
def __init__(self, env):
self.module = rave.RaveCreateModule(env, 'TraversabilityCPPUtility')
env.AddModule(self.module, '')
self.env = env
yamldict = yaml.safe_load(open(args.logfile))
# load environment
for kbdict in yamldict['environment']['kinbodies'].values():
name = kbdict['name']
uri = kbdict['uri']
# load/add kinbody
if kbdict['is_robot']:
if uri.endswith('.robot.xml'):
kb = env.ReadRobotXMLFile('robots/barrettwam.robot.xml')
kb.SetName(name)
env.Add(kb)
else:
print('uri:', uri)
urdf,srdf = uri.split()
m_urdf = openravepy.RaveCreateModule(env, 'urdf')
robot_name = m_urdf.SendCommand('Load {:s} {:s}'.format(urdf,srdf))
if robot_name != name:
raise RuntimeError('error, or_urdf name mismatch!')
kb = env.GetRobot(robot_name)
kb.SetActiveDOFs(kbdict['robot_state']['active_dof_indices'])
kb.SetActiveManipulator(kbdict['robot_state']['active_manipulator'])
else:
kb = e.ReadKinBodyXMLFile(uri)
kb.SetName(name)
env.Add(kb)
# transform
txdict = kbdict['kinbody_state']['transform']
orpose = txdict['orientation'] + txdict['position']
kb.SetTransform(orpose)
# dof values
import openravepy as rave
from openravepy.misc import InitOpenRAVELogging
import numpy as np
InitOpenRAVELogging()
env = rave.Environment()
env.SetViewer('qtcoin')
module = rave.RaveCreateModule(env, 'urdf')
name = module.SendCommand(
'load /home/ubuntu/catkin_ws/src/j2s7s300_standalone.urdf /home/ubuntu/catkin_ws/src/jaco7dof_standalonev1.srdf'
)
jaco = env.GetRobot(name)
# kb = env.GetKinBody(name)
#print(dir(jaco))
print(jaco.GetManipulators())
jaco.SetActiveManipulator("j2s7s300")
ikmodel = rave.databases.inversekinematics.InverseKinematicsModel(
jaco, iktype=rave.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
with env:
Tgoal = np.array([[0, -0.1, 0.0, -0.0], [-0.1, 0, 0, 0.0, 0.0],
[0.0, 0.0, -0.1, 0.0], [0.0, 0.0, 0.0, 1.0]])
sol = ikmodel.manip.FindIKSolution(
Tgoal,
rave.IkFilterOptions.CheckEnvCollisions) # get collision-free solution
with jaco: # save robot state
jaco.SetDOFValues(
def load_module(env, modname, modcmd=''):
m = openravepy.RaveCreateModule(env, modname)
if not m:
return None
env.AddModule(m, modcmd)
return m
import math
import numpy
import openravepy
import orcdchomp.orcdchomp
# start openrave
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
atexit.register(openravepy.RaveDestroy)
e = openravepy.Environment()
atexit.register(e.Destroy)
e.SetViewer('qtcoin')
# load the orcdchomp module
openravepy.RaveLoadPlugin('planner_plugin/build/chomp/chomp')
m_chomp = openravepy.RaveCreateModule(e, 'orcdchomp')
if not m_chomp:
raise RuntimeError('no chomp module found!')
orcdchomp.orcdchomp.bind(m_chomp)
# table
table = e.ReadKinBodyXMLFile('models/furniture/rolly-table.iv')
e.Add(table)
table.SetTransform([0.70711, 0.70711, 0, 0, 0, 0, 0])
# bottle (and its grasp)
mug = e.ReadKinBodyXMLFile('models/objects/mug3.iv')
e.Add(mug)
mug.SetTransform([1, 0, 0, 0, 0, 0, 0.7])
T_mug_palm = numpy.array([[0, -1, 0, 0.000], [0, 0, -1, 0.075],
[1, 0, 0, 0.100], [0, 0, 0, 1]])
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
atexit.register(openravepy.RaveDestroy)
env = openravepy.Environment()
atexit.register(env.Destroy)
robot = env.ReadRobotXMLFile('robots/barrettwam.robot.xml')
env.Add(robot)
robot.SetActiveManipulator('arm')
robot.SetActiveDOFs(robot.GetActiveManipulator().GetArmIndices())
# address this problem
q_start = [-2.0, -0.5, -0.2, -0.5, 0.0, 0.0, 0.0 ]
q_goal = [ 2.0, 0.5, 0.2, 0.5, 0.0, 0.0, 0.0 ]
family = openravepy.RaveCreateModule(env, 'Family')
env.Add(family, False, '--robot-name=BarrettWAM')
# save self-check
family.SendCommand('Let Self = $live')
# load up a box
box = openravepy.RaveCreateKinBody(env,'')
box.SetName('box')
aabbs = numpy.zeros((0,6))
aabb = numpy.array([[0,0,0, 0.1,0.1,0.1]])
aabbs = numpy.vstack((aabbs,aabb))
box.InitFromBoxes(aabbs,True)
env.Add(box)
box.SetTransform([1.,0.,0.,0., 0.,0.,0.75])
