def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description="Views a calibration pattern from multiple locations.")
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option(
"--scene",
action="store",
type="string",
dest="scene",
default="data/pa10calib.env.xml",
help="Scene file to load (default=%default)",
)
parser.add_option(
"--sensorname",
action="store",
type="string",
dest="sensorname",
default=None,
help="Name of the sensor whose views to generate (default is first sensor on robot)",
)
parser.add_option(
"--sensorrobot",
action="store",
type="string",
dest="sensorrobot",
default=None,
help="Name of the robot the sensor is attached to (default=%default)",
)
parser.add_option(
"--norandomize",
action="store_false",
dest="randomize",
default=True,
help="If set, will not randomize the bodies and robot position in the scene.",
)
parser.add_option(
"--novisibility",
action="store_false",
dest="usevisibility",
default=True,
help="If set, will not perform any visibility searching.",
)
parser.add_option(
"--posedist",
action="store",
type="float",
dest="posedist",
default=0.05,
help="An average distance between gathered poses. The smaller the value, the more poses robot will gather close to each other",
)
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options, defaultviewer=True)
main(env, options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Example shows how to query collision detection information using openravepy')
OpenRAVEGlobalArguments.addOptions(parser)
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description="test physics")
OpenRAVEGlobalArguments.addOptions(parser)
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
# set up planning environment
parser = OptionParser(description='Find the transform that moves the hand to target')
OpenRAVEGlobalArguments.addOptions(parser)
(options, leftargs) = parser.parse_args(args=args) # use default options
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) # the special setup for openrave tutorial
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Shows how to use different IK solutions for arms with few joints.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',action="store",type='string',dest='scene',default='data/pr2wam_test1.env.xml',
help='Scene file to load (default=%default)')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Explicitly specify goals to get a simple navigation and manipulation demo.', usage='openrave.py --example hanoi [options]')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--planner',action="store",type='string',dest='planner',default=None,
help='the planner to use')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description="test physics diff drive controller")
parser.add_option('--scene',action="store",type='string',dest='scene',default='data/diffdrive_sample.env.xml',
help='Scene file to load (default=%default)')
OpenRAVEGlobalArguments.addOptions(parser)
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Shows how to attach a callback to a viewer to perform functions.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',
action="store",type='string',dest='scene',default='data/lab1.env.xml',
help='OpenRAVE scene to load')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='RRT motion planning with constraints on the robot end effector.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',
action="store",type='string',dest='scene',default='data/lab1.env.xml',
help='Scene file to load (default=%default)')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Computes if an object is visibile inside the robot cameras.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',
action="store",type='string',dest='scene',default='data/testwamcamera.env.xml',
help='Scene file to load (default=%default)')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Displays all images of all camera sensors attached to a robot.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',
action="store",type='string',dest='scene',default='data/testwamcamera.env.xml',
help='OpenRAVE scene to load')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Example showing how to compute a valid grasp as fast as possible without computing a grasp set, this is used when the target objects change frequently.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene', action="store",type='string',dest='scene',default='data/wamtest1.env.xml',
help='Scene file to load (default=%default)')
parser.add_option('--manipname', action="store",type='string',dest='manipname',default=None,
help='Choose the manipulator to perform the grasping for')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Builds the convex decomposition of the robot and plots all the points that are tested inside of it.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--target', action="store",type='string',dest='target',default='robots/barrettwam.robot.xml',
help='Target body to load (default=%default)')
parser.add_option('--samplingdelta', action="store",type='float',dest='samplingdelta',default=None,
help='The sampling rate for the robot (default=%default)')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Visibility Planning Module.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',action="store",type='string',dest='scene',default='data/pa10grasp.env.xml',
help='openrave scene to load')
parser.add_option('--nocameraview',action="store_false",dest='usecameraview',default=True,
help='If set, will not open any camera views')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Manipulation planning example solving the hanoi problem.', usage='openrave.py --example hanoi [options]')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',action="store",type='string',dest='scene',default='data/hanoi_complex2.env.xml',
help='Scene file to load (default=%default)')
parser.add_option('--planner',action="store",type='string',dest='planner',default=None,
help='the planner to use')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Shows how to use the grasping.GraspingModel to compute valid grasps for manipulation.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene', action="store",type='string',dest='scene',default='data/wamtest1.env.xml',
help='Scene file to load (default=%default)')
parser.add_option('--manipname', action="store",type='string',dest='manipname',default=None,
help='Choose the manipulator to perform the grasping for')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description="test physics")
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',action="store",type='string',dest='scene',default='data/hanoi.env.xml',
help='Scene file to load (default=%default)')
parser.add_option('--timestep',action="store",type='float',dest='timestep',default=0.001,
help='The physics simulation time step size (default=%default)')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Simple cube assembly task using grasp sets.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',
action="store",type='string',dest='scene',default='data/hironxtable.env.xml',
help='Scene file to load (default=%default)')
parser.add_option('--manipname',
action="store",type='string',dest='manipname',default='leftarm_torso',
help='The manipulator to use')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
[docs]def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Move base where the robot can perform target grasp using inversereachability database.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--robot',action="store",type='string',dest='robot',default='robots/pr2-beta-static.zae',
help='Robot filename to use (default=%default)')
parser.add_option('--manipname',action="store",type='string',dest='manipname',default=None,
help='name of manipulator to use (default=%default)')
parser.add_option('--target',action="store",type='string',dest='target',default='data/mug2.kinbody.xml',
help='filename of the target to use (default=%default)')
(options, leftargs) = parser.parse_args(args=args) # use default options
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) # the special setup for openrave tutorial
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Shows how choose IK solutions so that move hand straight can move without discontinuities.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',
action="store",type='string',dest='scene',default='data/puma_tabletop.env.xml',
help='Scene file to load (default=%default)')
parser.add_option('--manipname',
action="store",type='string',dest='manipname',default=None,
help='Choose the manipulator to perform movement for')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Move base where the robot can perform target grasp using inversereachability database.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--robot',action="store",type='string',dest='robot',default='robots/pr2-beta-static.zae',
help='Robot filename to use (default=%default)')
parser.add_option('--manipname',action="store",type='string',dest='manipname',default=None,
help='name of manipulator to use (default=%default)')
parser.add_option('--target',action="store",type='string',dest='target',default='data/mug2.kinbody.xml',
help='filename of the target to use (default=%default)')
(options, leftargs) = parser.parse_args(args=args) # use default options
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) # the special setup for openrave tutorial
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Shows how to use different IK solutions for arms with few joints.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',action="store",type='string',dest='scene',default='data/katanatable.env.xml',
help='Scene file to load (default=%default)')
parser.add_option('--manipname',action="store",type='string',dest='manipname',default='arm',
help='name of manipulator to use (default=%default)')
parser.add_option('--withlocal',action="store_true",dest='withlocal',default=False,
help='If set, will use the TranslationLocalGlobal6D type to further specify the target point in the manipulator coordinate system')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Shows how to generate a 6D inverse kinematics solver and use it for getting all solutions.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',action="store",type='string',dest='scene',default='data/lab1.env.xml',
help='Scene file to load (default=%default)')
parser.add_option('--transparency',action="store",type='float',dest='transparency',default=0.8,
help='Transparency for every robot (default=%default)')
parser.add_option('--maxnumber',action="store",type='int',dest='maxnumber',default=10,
help='Max number of robots to render (default=%default)')
parser.add_option('--manipname',action="store",type='string',dest='manipname',default=None,
help='name of manipulator to use (default=%default)')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(description='Autonomous grasp and manipulation planning example.')
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option('--scene',
action="store",type='string',dest='scene',default='/home/user/testopenrave/data/mytest.env.xml',
help='Scene file to load (default=%default)')
parser.add_option('--nodestinations', action='store_true',dest='nodestinations',default=False,
help='If set, will plan without destinations.')
parser.add_option('--norandomize', action='store_false',dest='randomize',default=True,
help='If set, will not randomize the bodies and robot position in the scene.')
parser.add_option('--planner',action="store",type='string',dest='planner',default=None,
help='the planner to use')
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
main(env,options)
def run(args=None):
"""Command-line execution of the example.
:param args: arguments for script to parse, if not specified will use sys.argv
"""
parser = OptionParser(
description="Schunk Manipulation planning example\nFor a dual arm robot with Schunk LWA3 arms, plan trajectories for grasping an object and manipulating it on a shelf."
)
OpenRAVEGlobalArguments.addOptions(parser)
parser.add_option(
"--scene",
action="store",
type="string",
dest="scene",
default="data/dualarmmanipulation.env.xml",
help="Scene file to load",
)
(options, leftargs) = parser.parse_args(args=args)
env = OpenRAVEGlobalArguments.parseAndCreate(options, defaultviewer=True)
main(env, options)
def spin(self):
if True:
# Create a world object
world = ode.World()
# world.setGravity( (0,0,-3.81) )
world.setGravity( (0,0,0) )
CFM = world.getCFM()
ERP = world.getERP()
print "CFM: %s ERP: %s"%(CFM, ERP)
# world.setCFM(0.001)
# print "CFM: %s ERP: %s"%(CFM, ERP)
m_id = 0
self.pub_marker.eraseMarkers(0,3000, frame_id='world')
#
# Init Openrave
#
parser = OptionParser(description='Openrave Velma interface')
OpenRAVEGlobalArguments.addOptions(parser)
(options, leftargs) = parser.parse_args()
self.env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
# self.openrave_robot = self.env.ReadRobotXMLFile('robots/barretthand_ros.robot.xml')
mimic_joints = [
("right_HandFingerTwoKnuckleOneJoint", "right_HandFingerOneKnuckleOneJoint*1.0", "|right_HandFingerOneKnuckleOneJoint 1.0", ""),
("right_HandFingerOneKnuckleThreeJoint", "right_HandFingerOneKnuckleTwoJoint*0.33333", "|right_HandFingerOneKnuckleTwoJoint 0.33333", ""),
("right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerTwoKnuckleTwoJoint*0.33333", "|right_HandFingerTwoKnuckleTwoJoint 0.33333", ""),
("right_HandFingerThreeKnuckleThreeJoint", "right_HandFingerThreeKnuckleTwoJoint*0.33333", "|right_HandFingerThreeKnuckleTwoJoint 0.33333", ""),
]
rospack = rospkg.RosPack()
self.urdf_module = RaveCreateModule(self.env, 'urdf')
xacro_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.urdf.xml'
urdf_uri = '/tmp/barrett_hand.urdf'
srdf_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.srdf'
arg1 = "collision_model_full:=true"
arg2 = "collision_model_simplified:=false"
arg3 = "collision_model_enlargement:=0.0"
arg4 = "collision_model_no_hands:=false"
subprocess.call(["rosrun", "xacro", "xacro", "-o", urdf_uri, xacro_uri, arg1, arg2, arg3, arg4])
robot_name = self.urdf_module.SendCommand('load ' + urdf_uri + ' ' + srdf_uri )
print "robot name: " + robot_name
self.openrave_robot = self.env.GetRobot(robot_name)
self.env.Remove(self.openrave_robot)
for mimic in mimic_joints:
mj = self.openrave_robot.GetJoint(mimic[0])
mj.SetMimicEquations(0, mimic[1], mimic[2], mimic[3])
self.openrave_robot.GetActiveManipulator().SetChuckingDirection([0,0,0,0])#directions)
self.env.Add(self.openrave_robot)
self.openrave_robot = self.env.GetRobot(robot_name)
# print "robots: "
# print self.env.GetRobots()
joint_names = []
print "active joints:"
for j in self.openrave_robot.GetJoints():
joint_names.append(j.GetName())
print j
print "passive joints:"
for j in self.openrave_robot.GetPassiveJoints():
joint_names.append(j.GetName())
print j
# ODE does not support distance measure
self.env.GetCollisionChecker().SetCollisionOptions(CollisionOptions.Contacts)
self.env.Add(self.openrave_robot)
vertices, faces = surfaceutils.readStl(rospack.get_path('velma_scripts') + "/data/meshes/klucz_gerda_ascii.stl", scale=1.0)
# vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0)
self.addTrimesh("object", vertices, faces)
# self.addBox("object", 0.2,0.06,0.06)
# self.addSphere("object", 0.15)
# vertices, faces = self.getMesh("object")
#
# definition of the expected external wrenches
#
ext_wrenches = []
# origin of the external wrench (the end point of the key)
wr_orig = PyKDL.Vector(0.039, 0.0, 0.0)
for i in range(8):
# expected force at the end point
force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(0,1,0)
# expected torque at the com
torque = wr_orig * force
ext_wrenches.append(PyKDL.Wrench(force, torque))
# expected force at the end point
force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(-1,1,0)
# expected torque at the com
torque = wr_orig * force
ext_wrenches.append(PyKDL.Wrench(force, torque))
#
# definition of the grasps
#
grasp_id = 0
if grasp_id == 0:
grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [60.0/180.0*math.pi, None, None, None]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.122103662206, -0.124395758838, -0.702726011729, 0.689777190329), PyKDL.Vector(-0.00115787237883, -0.0194999426603, 0.458197712898))
# self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.174202588426, -0.177472708083, -0.691231954612, 0.678495061771), PyKDL.Vector(0.0, -0.0213436260819, 0.459123969078))
elif grasp_id == 1:
grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387771868, -0.708157209758, -0.0317875569224, 0.705090018033), PyKDL.Vector(4.65661287308e-10, 0.00145332887769, 0.472836345434))
elif grasp_id == 2:
grasp_direction = [0, 1, 0, 0] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, 90.0/180.0*math.pi, 0]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387763947, -0.708157179826, -0.0317875555789, 0.705089928626), PyKDL.Vector(0.0143095180392, 0.00145332887769, 0.483659058809))
elif grasp_id == 3:
grasp_direction = [0, 1, 1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00518634245761, -0.706548316769, -0.0182458505507, 0.707410947861), PyKDL.Vector(0.000126354629174, -0.00217361748219, 0.47637796402))
elif grasp_id == 4:
grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, 100.0/180.0*math.pi, None, 100.0/180.0*math.pi]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.153445252933, -0.161230275653, 0.681741576082, 0.696913201022), PyKDL.Vector(0.000126355327666, 0.00152841210365, 0.466048002243))
elif grasp_id == 5:
grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2
grasp_initial_configuration = [100.0/180.0*math.pi, 101.5/180.0*math.pi, None, 101.5/180.0*math.pi]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.155488650062, -0.159260521271, 0.690572597636, 0.688163302213), PyKDL.Vector(-0.000278688268736, 0.00575117766857, 0.461560428143))
elif grasp_id == 6:
grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2
grasp_initial_configuration = [90.0/180.0*math.pi, None, 0, 0]
self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.512641041738, -0.485843507183, -0.514213889193, 0.48655882699), PyKDL.Vector(-0.000278423947748, -0.00292747467756, 0.445628076792))
else:
print "ERROR: unknown grasp_id: %s"%(grasp_id)
exit(0)
self.updatePose("object", self.T_W_O)
with open('barret_hand_openrave2ros_joint_map2.txt', 'r') as f:
lines = f.readlines()
joint_map = {}
for line in lines:
line_s = line.split()
if len(line_s) == 2:
joint_map[line_s[0]] = line_s[1]
elif len(line_s) != 1:
print "error in joint map file"
exit(0)
print joint_map
self.pub_js = rospy.Publisher("/joint_states", JointState)
if True:
def getDirectionIndex(n):
min_angle = -45.0/180.0*math.pi
angle_range = 90.0/180.0*math.pi
angles_count = 5
angles_count2 = angles_count * angles_count
max_indices = angles_count2 * 6
if abs(n.x()) > abs(n.y()) and abs(n.x()) > abs(n.z()):
if n.x() > 0:
sec = 0
a1 = math.atan2(n.y(), n.x())
a2 = math.atan2(n.z(), n.x())
else:
sec = 1
a1 = math.atan2(n.y(), -n.x())
a2 = math.atan2(n.z(), -n.x())
elif abs(n.y()) > abs(n.x()) and abs(n.y()) > abs(n.z()):
if n.y() > 0:
sec = 2
a1 = math.atan2(n.x(), n.y())
a2 = math.atan2(n.z(), n.y())
else:
sec = 3
a1 = math.atan2(n.x(), -n.y())
a2 = math.atan2(n.z(), -n.y())
else:
if n.z() > 0:
sec = 4
a1 = math.atan2(n.x(), n.z())
a2 = math.atan2(n.y(), n.z())
else:
sec = 5
a1 = math.atan2(n.x(), -n.z())
a2 = math.atan2(n.y(), -n.z())
a1i = int(angles_count*(a1-min_angle)/angle_range)
a2i = int(angles_count*(a2-min_angle)/angle_range)
if a1i < 0:
print sec, a1i, a2i
a1i = 0
if a1i >= angles_count:
# print sec, a1i, a2i
a1i = angles_count-1
if a2i < 0:
print sec, a1i, a2i
a2i = 0
if a2i >= angles_count:
print sec, a1i, a2i
a2i = angles_count-1
return sec * angles_count2 + a1i * angles_count + a2i
# generate a dictionary of indexed directions
normals_sphere_indexed_dir = velmautils.generateNormalsSphere(3.0/180.0*math.pi)
print len(normals_sphere_indexed_dir)
indices_directions = {}
for n in normals_sphere_indexed_dir:
index = getDirectionIndex(n)
if not index in indices_directions:
indices_directions[index] = [n]
else:
indices_directions[index].append(n)
for index in indices_directions:
n_mean = PyKDL.Vector()
for n in indices_directions[index]:
n_mean += n
n_mean.Normalize()
indices_directions[index] = n_mean
# test direction indexing
if False:
normals_sphere = velmautils.generateNormalsSphere(3.0/180.0*math.pi)
print len(normals_sphere)
m_id = 0
for current_index in range(5*5*6):
count = 0
r = random.random()
g = random.random()
b = random.random()
for n in normals_sphere:
sec = -1
a1 = None
a2 = None
index = getDirectionIndex(n)
if index == current_index:
m_id = self.pub_marker.publishSinglePointMarker(n*0.1, m_id, r=r, g=g, b=b, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.007, 0.007, 0.007), T=None)
count += 1
print current_index, count
for index in indices_directions:
m_id = self.pub_marker.publishSinglePointMarker(indices_directions[index]*0.12, m_id, r=1, g=1, b=1, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.012, 0.012, 0.012), T=None)
exit(0)
#
# PyODE test
#
if True:
fixed_joints_names_for_fixed_DOF = [
["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread
["right_HandFingerOneKnuckleTwoJoint", "right_HandFingerOneKnuckleThreeJoint"], # f1
["right_HandFingerThreeKnuckleTwoJoint", "right_HandFingerThreeKnuckleThreeJoint"], # f3
["right_HandFingerTwoKnuckleTwoJoint", "right_HandFingerTwoKnuckleThreeJoint"], # f2
]
coupled_joint_names_for_fingers = ["right_HandFingerOneKnuckleThreeJoint", "right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerThreeKnuckleThreeJoint"]
actuated_joints_for_DOF = [
["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread
["right_HandFingerOneKnuckleTwoJoint"], # f1
["right_HandFingerThreeKnuckleTwoJoint"], # f3
["right_HandFingerTwoKnuckleTwoJoint"]] # f2
ignored_links = ["world", "world_link"]
self.run_ode_simulation = False
self.insert6DofGlobalMarker(self.T_W_O)
print "grasp_direction = [%s, %s, %s, %s] # spread, f1, f3, f2"%(grasp_direction[0], grasp_direction[1], grasp_direction[2], grasp_direction[3])
print "grasp_initial_configuration = [%s, %s, %s, %s]"%(grasp_initial_configuration[0], grasp_initial_configuration[1], grasp_initial_configuration[2], grasp_initial_configuration[3])
print "grasp_final_configuration = %s"%(self.openrave_robot.GetDOFValues())
rot_q = self.T_W_O.M.GetQuaternion()
print "self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s, %s, %s, %s), PyKDL.Vector(%s, %s, %s))"%(rot_q[0], rot_q[1], rot_q[2], rot_q[3], self.T_W_O.p.x(), self.T_W_O.p.y(), self.T_W_O.p.z())
self.erase6DofMarker()
raw_input("Press ENTER to continue...")
# reset the gripper in Openrave
self.openrave_robot.SetDOFValues([0,0,0,0])
# read the position of all links
grasp_links_poses = {}
for link in self.openrave_robot.GetLinks():
grasp_links_poses[link.GetName()] = self.OpenraveToKDL(link.GetTransform())
# print "obtained the gripper configuration for the grasp:"
# print finalconfig[0]
#
# simulation in ODE
#
# Create a world object
world = ode.World()
# world.setGravity( (0,0,-3.81) )
world.setGravity( (0,0,0) )
CFM = world.getCFM()
ERP = world.getERP()
print "CFM: %s ERP: %s"%(CFM, ERP)
# world.setCFM(0.001)
# print "CFM: %s ERP: %s"%(CFM, ERP)
self.space = ode.Space()
# Create a body inside the world
body = ode.Body(world)
M = ode.Mass()
M.setCylinderTotal(0.02, 1, 0.005, 0.09)
body.setMass(M)
ode_mesh = ode.TriMeshData()
ode_mesh.build(vertices, faces)
geom = ode.GeomTriMesh(ode_mesh, self.space)
geom.name = "object"
geom.setBody(body)
self.setOdeBodyPose(geom, self.T_W_O)
ode_gripper_geoms = {}
for link in self.openrave_robot.GetLinks():
link_name = link.GetName()
if link_name in ignored_links:
print "ignoring: %s"%(link_name)
continue
print "adding: %s"%(link_name)
ode_mesh_link = None
body_link = None
T_W_L = self.OpenraveToKDL(link.GetTransform())
col = link.GetCollisionData()
vertices = col.vertices
faces = col.indices
ode_mesh_link = ode.TriMeshData()
ode_mesh_link.build(vertices, faces)
ode_gripper_geoms[link_name] = ode.GeomTriMesh(ode_mesh_link, self.space)
if True:
body_link = ode.Body(world)
M_link = ode.Mass()
M_link.setCylinderTotal(0.05, 1, 0.01, 0.09)
body_link.setMass(M_link)
ode_gripper_geoms[link_name].setBody(body_link)
ode_gripper_geoms[link_name].name = link.GetName()
self.setOdeBodyPose(body_link, T_W_L)
actuated_joint_names = []
for dof in range(4):
for joint_name in actuated_joints_for_DOF[dof]:
actuated_joint_names.append(joint_name)
ode_gripper_joints = {}
for joint_name in joint_names:
joint = self.openrave_robot.GetJoint(joint_name)
link_parent = joint.GetHierarchyParentLink().GetName()
link_child = joint.GetHierarchyChildLink().GetName()
if link_parent in ode_gripper_geoms and link_child in ode_gripper_geoms:
axis = joint.GetAxis()
anchor = joint.GetAnchor()
if joint_name in actuated_joint_names:
ode_gripper_joints[joint_name] = ode.AMotor(world)
ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody())
ode_gripper_joints[joint_name].setMode(ode.AMotorUser)#Euler)
# ode_gripper_joints[joint_name].setNumAxes(3)
ode_gripper_joints[joint_name].setAxis(0,1,axis)
# ode_gripper_joints[joint_name].setAxis(1,1,axis)
# ode_gripper_joints[joint_name].setAxis(2,1,axis)
ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0)
# ode_gripper_joints[joint_name].setParam(ode.ParamFMax2, 10.0)
# ode_gripper_joints[joint_name].setParam(ode.ParamFMax3, 10.0)
else:
ode_gripper_joints[joint_name] = ode.HingeJoint(world)
ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody())
ode_gripper_joints[joint_name].setAxis(-axis)
ode_gripper_joints[joint_name].setAnchor(anchor)
# ode_gripper_joints[joint_name] = ode.HingeJoint(world)
# ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody())
# ode_gripper_joints[joint_name].setAxis(-axis)
# ode_gripper_joints[joint_name].setAnchor(anchor)
# if joint_name in actuated_joint_names:
# ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0)
limits = joint.GetLimits()
value = joint.GetValue(0)
# print joint_name
# print "limits: %s %s"%(limits[0], limits[1])
# print "axis: %s"%(axis)
# print "anchor: %s"%(anchor)
# print "value: %s"%(value)
lim = [limits[0], limits[1]]
if limits[0] <= -math.pi:
# print "lower joint limit %s <= -PI, setting to -PI+0.01"%(limits[0])
lim[0] = -math.pi + 0.01
if limits[1] >= math.pi:
# print "upper joint limit %s >= PI, setting to PI-0.01"%(limits[1])
lim[1] = math.pi - 0.01
ode_gripper_joints[joint_name].setParam(ode.ParamLoStop, lim[0])
ode_gripper_joints[joint_name].setParam(ode.ParamHiStop, lim[1])
# ode_gripper_joints[joint_name].setParam(ode.ParamFudgeFactor, 0.5)
ode_fixed_joint = ode.FixedJoint(world)
ode_fixed_joint.attach(None, ode_gripper_geoms["right_HandPalmLink"].getBody())
ode_fixed_joint.setFixed()
#
# set the poses of all links as for the grasp
#
for link_name in grasp_links_poses:
if link_name in ignored_links:
continue
ode_body = ode_gripper_geoms[link_name].getBody()
T_W_L = grasp_links_poses[link_name]
self.setOdeBodyPose(ode_body, T_W_L)
fixed_joint_names = []
fixed_joint_names += coupled_joint_names_for_fingers
for dof in range(4):
if grasp_direction[dof] == 0.0:
for joint_name in fixed_joints_names_for_fixed_DOF[dof]:
if not joint_name in fixed_joint_names:
fixed_joint_names.append(joint_name)
#
# change all coupled joints to fixed joints
#
fixed_joints = {}
for joint_name in fixed_joint_names:
# save the bodies attached
body0 = ode_gripper_joints[joint_name].getBody(0)
body1 = ode_gripper_joints[joint_name].getBody(1)
# save the joint angle
if joint_name in actuated_joint_names:
angle = ode_gripper_joints[joint_name].getAngle(0)
else:
angle = ode_gripper_joints[joint_name].getAngle()
# detach the joint
ode_gripper_joints[joint_name].attach(None, None)
del ode_gripper_joints[joint_name]
fixed_joints[joint_name] = [ode.FixedJoint(world), angle]
fixed_joints[joint_name][0].attach(body0, body1)
fixed_joints[joint_name][0].setFixed()
# change all actuated joints to motor joints
# actuated_joint_names = []
# for dof in range(4):
# for joint_name in actuated_joints_for_DOF[dof]:
# actuated_joint_names.append(joint_name)
# for joint_name in actuated_joint_names:
# A joint group for the contact joints that are generated whenever
# two bodies collide
contactgroup = ode.JointGroup()
print "ode_gripper_geoms:"
print ode_gripper_geoms
initial_T_W_O = self.T_W_O
# Do the simulation...
dt = 0.001
total_time = 0.0
failure = False
#
# PID
#
Kc = 1.0
# Ti = 1000000000000.0
KcTi = 0.0
Td = 0.0
Ts = 0.001
pos_d = 1.0
e0 = 0.0
e1 = 0.0
e2 = 0.0
u = 0.0
u_max = 1.0
while total_time < 5.0 and not rospy.is_shutdown():
#
# ODE simulation
#
joint = ode_gripper_joints["right_HandFingerOneKnuckleTwoJoint"]
e2 = e1
e1 = e0
e0 = pos_d - joint.getAngle(0)
u = u + Kc * (e0 - e1) + KcTi * Ts * e0 + Kc * Td / Ts * (e0 - 2.0 * e1 + e2)
if u > u_max:
u = u_max
if u < -u_max:
u = -u_max
joint.setParam(ode.ParamFMax, 10000.0)
joint.setParam(ode.ParamVel, 1.1)
# joint.setParam(ode.ParamVel2, 1.1)
# joint.setParam(ode.ParamVel3, 1.1)
print joint.getAngle(0), " ", u #, " ", joint.getAngleRate(0)
# print u
# joint.addTorque(u)
# for dof in range(4):
# for joint_name in actuated_joints_for_DOF[dof]:
# if joint_name in ode_gripper_joints:
# ode_gripper_joints[joint_name].addTorque(1*grasp_direction[dof])
self.grasp_contacts = []
self.space.collide((world,contactgroup), self.near_callback)
world.step(dt)
total_time += dt
contactgroup.empty()
#
# ROS interface
#
old_m_id = m_id
m_id = 0
# publish frames from ODE
if False:
for link_name in ode_gripper_geoms:
link_body = ode_gripper_geoms[link_name].getBody()
if link_body == None:
link_body = ode_gripper_geoms[link_name]
T_W_Lsim = self.getOdeBodyPose(link_body)
m_id = self.pub_marker.publishFrameMarker(T_W_Lsim, m_id, scale=0.05, frame='world', namespace='default')
# publish the mesh of the object
T_W_Osim = self.getOdeBodyPose(body)
m_id = self.pub_marker.publishConstantMeshMarker("package://velma_scripts/data/meshes/klucz_gerda_binary.stl", m_id, r=1, g=0, b=0, scale=1.0, frame_id='world', namespace='default', T=T_W_Osim)
# update the gripper visualization in ros
js = JointState()
js.header.stamp = rospy.Time.now()
for jn in joint_map:
ros_joint_name = joint_map[jn]
js.name.append(ros_joint_name)
if jn in ode_gripper_joints:
if jn in actuated_joint_names:
js.position.append( ode_gripper_joints[jn].getAngle(0) )
else:
js.position.append( ode_gripper_joints[jn].getAngle() )
elif jn in fixed_joints:
js.position.append(fixed_joints[jn][1])
else:
js.position.append(0)
self.pub_js.publish(js)
# draw contacts
for c in self.grasp_contacts:
cc = PyKDL.Vector(c[0], c[1], c[2])
cn = PyKDL.Vector(c[3], c[4], c[5])
m_id = self.pub_marker.publishVectorMarker(cc, cc+cn*0.04, m_id, 1, 0, 0, frame='world', namespace='default', scale=0.003)
if m_id < old_m_id:
self.pub_marker.eraseMarkers(m_id,old_m_id+1, frame_id='world')
diff_T_W_O = PyKDL.diff(initial_T_W_O, T_W_Osim)
# if diff_T_W_O.vel.Norm() > 0.02 or diff_T_W_O.rot.Norm() > 30.0/180.0*math.pi:
# print "the object moved"
# print diff_T_W_O
# failure = True
# break
rospy.sleep(0.01)
if not failure:
T_O_Wsim = T_W_Osim.Inverse()
TR_O_Wsim = PyKDL.Frame(T_O_Wsim.M)
contacts = []
for c in self.grasp_contacts:
cc_W = PyKDL.Vector(c[0], c[1], c[2])
cn_W = PyKDL.Vector(c[3], c[4], c[5])
cc_O = T_O_Wsim * cc_W
cn_O = TR_O_Wsim * cn_W
contacts.append([cc_O[0], cc_O[1], cc_O[2], cn_O[0], cn_O[1], cn_O[2]])
gws, contact_planes = self.generateGWS(contacts, 1.0)
grasp_quality = None
for wr in ext_wrenches:
wr_qual = self.getQualityMeasure2(gws, wr)
if grasp_quality == None or wr_qual < grasp_quality:
grasp_quality = wr_qual
grasp_quality_classic = self.getQualityMeasure(gws)
print "grasp_quality_classic: %s grasp_quality: %s"%(grasp_quality_classic, grasp_quality)
exit(0)
def spin(self):
parser = OptionParser(description='Openrave Velma interface')
OpenRAVEGlobalArguments.addOptions(parser)
(options, leftargs) = parser.parse_args()
self.env = OpenRAVEGlobalArguments.parseAndCreate(options)#,defaultviewer=True)
self.openrave_robot = self.env.ReadRobotXMLFile('robots/barretthand_ros.robot.xml')
link = self.openrave_robot.GetLink("right_HandFingerOneKnuckleThreeLink")
col = link.GetCollisionData()
vertices = col.vertices
faces = col.indices
print "sampling the surface..."
points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.001)
print "surface has %s points"%(len(points))
# vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0)
# points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.002)
print len(points)
with open('points.txt', 'w') as f:
for pt in points:
p = pt.pos
f.write(str(p.x()) + ' ' + str(p.y()) + ' ' + str(p.z()) + '\n')
call(["touch","out_surf.off"])
call(["touch","out_axis.off"])
call(["/home/dseredyn/code/cocone/tightcocone/tcocone-linux", "-m", "points.txt", "out"])
calc_radius = False
points_medial = []
with open('out_axis.off', 'r') as f:
header = f.readline()
header_s = header.split()
p_count = int(header_s[1])
for i in range(0, p_count):
line = f.readline()
values = line.split()
center = PyKDL.Vector(float(values[0]), float(values[1]), float(values[2]))
if calc_radius:
radius = None
for pt in points:
dist = (center-pt.pos).Norm()
if radius == None or radius > dist:
radius = dist
points_medial.append([center, radius*2.0])
else:
points_medial.append([center, 0.001])
print len(points_medial)
# m_id = 0
# for p in points:
# m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(p[0], p[1], p[2]), m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.005, 0.005, 0.005), T=None)
# m_id = 0
# for p in points_medial:
# m_id = self.pub_marker.publishSinglePointMarker(p[0], m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(p[1],p[1],p[1]), T=None)
m_id = 0
m_id = self.pub_marker.publishMultiPointsMarkerWithSize(points_medial, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, T=None)
# m_id = self.pub_marker.publishMultiPointsMarker(points_medial, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.0005, 0.0005, 0.0005), T=None)
rospy.sleep(1)
exit(0)
dest='collision_map',
default='/collision_map/collision_map',
help=
'The collision map topic (maping_msgs/CollisionMap), by (default=%default)'
)
parser.add_option(
'--mapframe',
action="store",
type='string',
dest='mapframe',
default=None,
help=
'The frame of the map used to position the robot. If --mapframe="" is specified, then nothing will be transformed with tf'
)
(options, args) = parser.parse_args()
env = OpenRAVEGlobalArguments.parseAndCreate(options, defaultviewer=True)
RaveLoadPlugin(
os.path.join(roslib.packages.get_pkg_dir('orrosplanning'), 'lib',
'liborrosplanning.so'))
RaveLoadPlugin(
os.path.join(roslib.packages.get_pkg_dir('openraveros'), 'lib',
'openraveros'))
namespace = 'openrave'
env.AddModule(RaveCreateModule(env, 'rosserver'), namespace)
# load the orrosplanning plugin
print 'initializing, please wait for ready signal...'
try:
rospy.init_node('ik_openrave', disable_signals=False)
with env:
def spin(self):
parser = OptionParser(description='Openrave Velma interface')
OpenRAVEGlobalArguments.addOptions(parser)
(options, leftargs) = parser.parse_args()
self.env = OpenRAVEGlobalArguments.parseAndCreate(
options) #,defaultviewer=True)
self.openrave_robot = self.env.ReadRobotXMLFile(
'robots/barretthand_ros.robot.xml')
link = self.openrave_robot.GetLink(
"right_HandFingerOneKnuckleThreeLink")
col = link.GetCollisionData()
vertices = col.vertices
faces = col.indices
print "sampling the surface..."
points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.001)
print "surface has %s points" % (len(points))
# vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0)
# points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.002)
print len(points)
with open('points.txt', 'w') as f:
for pt in points:
p = pt.pos
f.write(
str(p.x()) + ' ' + str(p.y()) + ' ' + str(p.z()) + '\n')
call(["touch", "out_surf.off"])
call(["touch", "out_axis.off"])
call([
"/home/dseredyn/code/cocone/tightcocone/tcocone-linux", "-m",
"points.txt", "out"
])
calc_radius = False
points_medial = []
with open('out_axis.off', 'r') as f:
header = f.readline()
header_s = header.split()
p_count = int(header_s[1])
for i in range(0, p_count):
line = f.readline()
values = line.split()
center = PyKDL.Vector(float(values[0]), float(values[1]),
float(values[2]))
if calc_radius:
radius = None
for pt in points:
dist = (center - pt.pos).Norm()
if radius == None or radius > dist:
radius = dist
points_medial.append([center, radius * 2.0])
else:
points_medial.append([center, 0.001])
print len(points_medial)
# m_id = 0
# for p in points:
# m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(p[0], p[1], p[2]), m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.005, 0.005, 0.005), T=None)
# m_id = 0
# for p in points_medial:
# m_id = self.pub_marker.publishSinglePointMarker(p[0], m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(p[1],p[1],p[1]), T=None)
m_id = 0
m_id = self.pub_marker.publishMultiPointsMarkerWithSize(
points_medial,
m_id,
r=1,
g=0,
b=0,
namespace='default',
frame_id='world',
m_type=Marker.SPHERE,
T=None)
# m_id = self.pub_marker.publishMultiPointsMarker(points_medial, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.0005, 0.0005, 0.0005), T=None)
rospy.sleep(1)
exit(0)
plugins = RaveGetPluginInfo()
interfacenames = dict()
for type in InterfaceType.values.values():
interfacenames[type] = []
for pluginname, info in plugins:
for type, names in info.interfacenames:
interfacenames[type] += [(n, pluginname) for n in names]
print 'Number of plugins: %d' % len(plugins)
for type, names in interfacenames.iteritems():
print '%s: %d' % (str(type), len(names))
names.sort()
for interfacename, pluginname in names:
print ' %s - %s' % (interfacename, pluginname)
RaveDestroy()
sys.exit(0)
env, viewername = OpenRAVEGlobalArguments.parseAndCreate(
options, defaultviewer=True, returnviewer=True)
try:
# load files after viewer is loaded since they may contain information about where to place the camera
for arg in args:
env.Load(arg)
if options.pythoncmd is not None:
eval(compile(options.pythoncmd, '<string>', 'exec'))
viewer = None
if viewername is not None:
if sys.platform.startswith('darwin'):
viewer = RaveCreateViewer(env, viewername)
env.Add(viewer)
else:
# create in a separate thread for windows and linux since the signals do not get messed up
env.SetViewer(viewername)
parser.add_option('--ipython', '-i',action="store_true",dest='ipython',default=False,
help='if true will drop into the ipython interpreter rather than spin')
parser.add_option('--mapframe',action="store",type='string',dest='mapframe',default=None,
help='The frame of the map used to position the robot. If --mapframe="" is specified, then nothing will be transformed with tf')
parser.add_option('--jitter',action="store",type='float',dest='jitter',default=None,
help='The jitter to use when moving robot out of collision')
parser.add_option('--maxvelmult',action='store',type='float',dest='maxvelmult',default=1.0,
help='The maximum velocity multiplier when timing the trajectories (default=%default)')
parser.add_option('--wait-for-collisionmap',action='store',type='float',dest='wait_for_collisionmap',default=None,
help='Time (seconds) to set. Will wait for the collision map time stamp to reach the same time as the service being called. If 0, will wait indefinitely.')
parser.add_option('--request-for-joint_states',action='store',type='string',dest='request_for_joint_states',default='topic',
help='whether to get joint states from topic or service. If ="service", will not update robot joint states until receiving service call.')
parser.add_option('--use-simulation',action='store',type='string',dest='simulation',default=None,
help='if use-simulation is set, we dismiss timestamp of collisionmap.')
(options, args) = parser.parse_args()
env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=False)
RaveLoadPlugin(os.path.join(roslib.packages.get_pkg_dir('orrosplanning'),'lib','liborrosplanning.so'))
#RaveLoadPlugin(os.path.join(roslib.packages.get_pkg_dir('openraveros'),'lib','openraveros'))
namespace = 'openrave'
#env.AddModule(RaveCreateModule(env,'rosserver'),namespace)
rospy.loginfo('initializing, please wait for ready signal...')
handles = [] # for viewer
def UpdateRobotJoints(msg):
global options
with envlock:
with env:
for name,pos in izip(msg.name,msg.position):
j = robot.GetJoint(name)
if j is not None:
values[j.GetDOFIndex()] = pos
robot.SetDOFValues(values)
