def centered_box_body(env, dx, dy, dz, name=None, color=None, transparency=None): body = RaveCreateKinBody(env, '') body.InitFromBoxes(np.array([[0, 0, 0, .5*dx, .5*dy, .5*dz]]), draw=True) if name is not None: set_name(body, name) if color is not None: set_color(body, color) if transparency is not None: set_transparency(body, transparency) return body
def main(env,options):
"Main example code."
if options.iktype is not None:
# cannot use .names due to python 2.5 (or is it boost version?)
for value,type in IkParameterization.Type.values.iteritems():
if type.name.lower() == options.iktype.lower():
iktype = type
break
else:
iktype = IkParameterizationType.Transform6D
env.Load(options.scene)
with env:
# load the Transform6D IK models
ikmodels = []
for robot in env.GetRobots():
for manip in robot.GetManipulators():
print manip
try:
robot.SetActiveManipulator(manip)
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype)
if not ikmodel.load():
ikmodel.autogenerate()
if not ikmodel.has():
continue
ikmodels.append(ikmodel)
except Exception, e:
print 'failed manip %s'%manip, e
if len(ikmodels) == 0:
raveLogWarn('no manipulators found that can be loaded with iktype %s'%str(iktype))
# create the pickers
pickers = []
for ikmodel in ikmodels:
raveLogInfo('creating picker for %s'%str(ikmodel.manip))
picker = RaveCreateKinBody(env,'')
picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True)
for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
color = zeros(3)
color[igeom] = 1
geom.SetDiffuseColor(color)
picker.SetName(ikmodel.manip.GetName())
env.Add(picker,True)
# have to disable since not part of collision
picker.Enable(False)
picker.SetTransform(ikmodel.manip.GetTransform())
pickers.append([picker,ikmodel.manip])
def main(env,options):
"Main example code."
if options.iktype is not None:
# cannot use .names due to python 2.5 (or is it boost version?)
for value,type in IkParameterization.Type.values.items():
if type.name.lower() == options.iktype.lower():
iktype = type
break
else:
iktype = IkParameterizationType.Transform6D
env.Load(options.scene)
with env:
# load the Transform6D IK models
ikmodels = []
for robot in env.GetRobots():
for manip in robot.GetManipulators():
print(manip)
try:
robot.SetActiveManipulator(manip)
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype)
if not ikmodel.load():
ikmodel.autogenerate()
if not ikmodel.has():
continue
ikmodels.append(ikmodel)
except Exception as e:
print('failed manip %s %r'%(manip, e))
if len(ikmodels) == 0:
raveLogWarn('no manipulators found that can be loaded with iktype %s'%str(iktype))
# create the pickers
pickers = []
for ikmodel in ikmodels:
raveLogInfo('creating picker for %s'%str(ikmodel.manip))
picker = RaveCreateKinBody(env,'')
picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True)
for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
color = zeros(3)
color[igeom] = 1
geom.SetDiffuseColor(color)
picker.SetName(ikmodel.manip.GetName())
env.Add(picker,True)
# have to disable since not part of collision
picker.Enable(False)
picker.SetTransform(ikmodel.manip.GetTransform())
pickers.append([picker,ikmodel.manip])
raveLogInfo('pickers loaded, try moving them')
def PickerThread(env,pickers,iktype):
"""this function runs in a separate thread monitoring each of the pickers
"""
Tpickers = [None]*len(pickers)
while True:
if env.GetViewer() is None:
break
with env:
for ipicker,(picker,manip) in enumerate(pickers):
T=picker.GetTransform()
if Tpickers[ipicker] is not None and sum(abs(Tpickers[ipicker]-T).flatten()) <= 1e-10:
continue
data = None
if iktype == IkParameterizationType.Direction3D:
data = T[0:3,2]
elif iktype == IkParameterizationType.Lookat3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.Ray4D:
data = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.Rotation3D:
data = T[0:3,0:3]
elif iktype == IkParameterizationType.Transform6D:
data = T
elif iktype == IkParameterizationType.Translation3D:
data = T[0:3,3]
elif iktype == IkParameterizationType.TranslationDirection5D:
ikparam = Ray(T[0:3,3],T[0:3,2])
elif iktype == IkParameterizationType.TranslationLocalGlobal6D:
ikparam = [T[0:3,3],zeros(3)]
else:
raveLogWarn('iktype %s unsupported'%str(iktype))
continue
sol = manip.FindIKSolution(IkParameterization(data,iktype),IkFilterOptions.CheckEnvCollisions)
if sol is not None:
robot.SetDOFValues(sol,manip.GetArmIndices())
# have to update all other pickers since two manipulators can be connected to the same joints (torso)
for ipicker2, (picker2,manip2) in enumerate(pickers):
Tpickers[ipicker2] = manip2.GetTransform()
picker2.SetTransform(manip2.GetTransform())
# changing color produces bugs with qtcoin
# for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
# color = zeros(3)
# color[igeom] = 0.4 if sol is None else 1.0
# geom.SetDiffuseColor(color)
Tpickers[ipicker] = T
# update rate of 0.05s
time.sleep(0.05)
# create the thread
t = threading.Thread(target=PickerThread,args=[env,pickers,iktype])
t.start()
while True:
t.join(1)
