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 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 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():
'''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 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 initialize(attach_viewer=False, sim=True, user_id='human', env=None):
"""Initialize the Human Robot"""
prpy.logger.initialize_logging()
if not env:
env = Environment()
#Setup Manipulators
with env:
robot = env.ReadKinBodyXMLFile('robots/man1.dae')
robot.SetName(user_id) #needed in order to have different humans in the same env
env.AddKinBody(robot)
robot.left_arm = robot.GetManipulator('leftarm')
robot.left_arm.hand = robot.left_arm.GetEndEffector()
robot.left_hand = robot.left_arm.hand
robot.right_arm = robot.GetManipulator('rightarm')
robot.right_arm.hand = robot.right_arm.GetEndEffector()
robot.right_hand = robot.right_arm.hand
bind_subclass(robot, Robot, robot_name=user_id)
bind_subclass(robot.left_arm, Manipulator)
bind_subclass(robot.right_arm, Manipulator)
bind_subclass(robot.left_arm.hand, HumanHand, manipulator=robot.left_arm, sim=True)
bind_subclass(robot.right_arm.hand, HumanHand, manipulator=robot.right_arm, sim=True)
#Setup Controller
args = 'IdealController'
affine_dofs=0
delegate_controllerr = RaveCreateController(env, args)
if delegate_controllerr is None:
message = 'Creating controller {0:s} of type {1:s} failed.'.format(robot.right_arm.GetName(), args)
raise openrave_exception(message)
robot.multicontroller.AttachController(delegate_controllerr, robot.right_arm.GetArmIndices(), affine_dofs)
delegate_controllerl = RaveCreateController(env, args)
if delegate_controllerl is None:
message = 'Creating controller {0:s} of type {1:s} failed.'.format(robot.left_arm.GetName(), args)
raise openrave_exception(message)
robot.multicontroller.AttachController(delegate_controllerl, robot.left_arm.GetArmIndices(), affine_dofs)
#Setup IK
ikmodel_left = InverseKinematicsModel(
robot,
iktype=IkParameterizationType.Transform6D,
manip=robot.left_arm)
if not ikmodel_left.load():
ikmodel_left.autogenerate()
ikmodel_right = InverseKinematicsModel(
robot,
iktype=IkParameterizationType.Transform6D,
manip=robot.right_arm)
if not ikmodel_right.load():
ikmodel_right.autogenerate()
#Setup planning pipeline
robot.planner = Sequence(
SnapPlanner(),
VectorFieldPlanner(),
CBiRRTPlanner())
robot.simplifier = None
robot.retimer = HauserParabolicSmoother() # hack
robot.smoother = HauserParabolicSmoother()
robot.actions = prpy.action.ActionLibrary()
#Setup viewer. Default to load rviz
if env.GetViewer() is None:
if attach_viewer == True:
attach_viewer = 'rviz'
env.SetViewer(attach_viewer)
#Fallback on qtcoin if rviz couldnt load
if env.GetViewer is None:
logger.warning('Loading the Rviz viewer failed. 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 ROS Logging since loading Rviz might have added it
prpy.logger.remove_ros_logger()
#changing orientation
with env:
robotLocation = numpy.array([[ -1. , 0. , 0. , 0.],
[ 0. , 0. , 1. , 0.],
[ 0. , 1. , 0. , 0.],
[ 0. , 0. , 0. , 1. ]])
robot.SetTransform(robotLocation)
return env, robot
