def add_to_vis(self,robot=None,animate=True,base_xform=None):
"""Adds the gripper to the klampt.vis scene."""
from klampt import vis
from klampt import WorldModel,Geometry3D,GeometricPrimitive
from klampt.model.trajectory import Trajectory
prefix = "gripper_"+self.name
if robot is None and self.klampt_model is not None:
w = WorldModel()
if w.readFile(self.klampt_model):
robot = w.robot(0)
vis.add(prefix+"_gripper",w)
robotPath = (prefix+"_gripper",robot.getName())
elif robot is not None:
vis.add(prefix+"_gripper",robot)
robotPath = prefix+"_gripper"
if robot is not None:
assert self.base_link >= 0 and self.base_link < robot.numLinks()
robot.link(self.base_link).appearance().setColor(1,0.75,0.5)
if base_xform is None:
base_xform = robot.link(self.base_link).getTransform()
else:
if robot.link(self.base_link).getParent() >= 0:
print("Warning, setting base link transform for an attached gripper base")
#robot.link(self.base_link).setParent(-1)
robot.link(self.base_link).setParentTransform(*base_xform)
robot.setConfig(robot.getConfig())
for l in self.finger_links:
assert l >= 0 and l < robot.numLinks()
robot.link(l).appearance().setColor(1,1,0.5)
if robot is not None and animate:
q0 = robot.getConfig()
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(1)
else:
robot.driver(i).setValue(1)
traj = Trajectory([0],[robot.getConfig()])
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(0)
traj.times.append(traj.endTime()+0.5)
traj.milestones.append(robot.getConfig())
else:
robot.driver(i).setValue(0)
traj.times.append(traj.endTime()+1)
traj.milestones.append(robot.getConfig())
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
assert len(traj.times) == len(traj.milestones)
traj.checkValid()
vis.animate(robotPath,traj)
robot.setConfig(q0)
if self.center is not None:
vis.add(prefix+"_center",se3.apply(base_xform,self.center))
center_point = (0,0,0) if self.center is None else self.center
outer_point = (0,0,0)
if self.primary_axis is not None:
length = 0.1 if self.finger_length is None else self.finger_length
outer_point = vectorops.madd(self.center,self.primary_axis,length)
line = Trajectory([0,1],[self.center,outer_point])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_primary",line,color=(1,0,0,1))
if self.secondary_axis is not None:
width = 0.1 if self.maximum_span is None else self.maximum_span
line = Trajectory([0,1],[vectorops.madd(outer_point,self.secondary_axis,-0.5*width),vectorops.madd(outer_point,self.secondary_axis,0.5*width)])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_secondary",line,color=(0,1,0,1))
elif self.maximum_span is not None:
#assume vacuum gripper?
p = GeometricPrimitive()
p.setSphere(outer_point,self.maximum_span)
g = Geometry3D()
g.set(p)
vis.add(prefix+"_opening",g,color=(0,1,0,0.25))
class GeneralizedPath:
def __init__(self):
self.type = None
self.data = None
def load(self, fn):
if fn.endswith('xml'):
self.type = 'MultiPath'
self.data = MultiPath()
self.data.load(fn)
elif fn.endswith('path') or fn.endswith('traj'):
self.type = 'Trajectory'
self.data = Trajectory()
self.data.load(fn)
elif fn.endswith('configs') or fn.endswith('milestones'):
self.type = 'Configs'
self.data = Trajectory()
self.data.configs = loader.load('Configs', fn)
self.data.times = list(range(len(self.data.configs)))
else:
print("Unknown extension on file", fn, ", loading as Trajectory")
self.type = 'Trajectory'
self.data = Trajectory()
self.data.load(fn)
def save(self, fn):
if fn.endswith('xml') and self.type != 'MultiPath':
self.toMultipath()
self.data.save(fn)
def hasTiming(self):
if self.type == 'Configs':
return False
if self.type == 'Trajectory':
return True
return self.data.hasTiming()
def duration(self):
if self.type == 'MultiPath':
return self.data.endTime()
else:
return self.data.duration()
def start(self):
if self.type == 'MultiPath':
return self.data.sections[0].configs[0]
else:
return self.data.milestones[0]
def end(self):
if self.type == 'MultiPath':
return self.data.sections[-1].configs[-1]
else:
return self.data.milestones[-1]
def timescale(self, scale, ofs):
rev = (scale < 0)
if rev:
scale = -scale
if self.type == 'MultiPath':
if not self.data.hasTiming():
print("MultiPath timescale(): Path does not have timing")
return
et = self.data.endTime()
for s in self.data.sections:
s.times = [ofs + scale * (et - t) for t in s.times]
s.times.reverse()
s.configs.reverse()
if s.velocities:
s.velocities.reverse()
self.data.sections.reverse()
else:
et = self.data.times[-1]
self.data.times = [
ofs + scale * (et - t) for t in self.data.times
]
self.data.times.reverse()
self.data.milestones.reverse()
if self.type == 'Configs':
if scale != 1 or ofs != 0:
self.type = 'Trajectory'
else:
if self.type == 'MultiPath':
if not self.hasTiming():
print("MultiPath timescale(): Path does not have timing")
return
for s in self.data.sections:
s.times = [ofs + scale * t for t in s.times]
else:
self.data.times = [ofs + scale * t for t in self.data.times]
if self.type == 'Configs':
if scale != 1 or ofs != 0:
self.type = 'Trajectory'
def toMultipath(self):
if self.type == 'MultiPath':
return
else:
traj = self.data
mpath = MultiPath()
mpath.sections.append(MultiPath.Section())
mpath.sections[0].times = traj.times
mpath.sections[0].configs = traj.milestones
self.data = mpath
self.type = 'MultiPath'
def toTrajectory(self):
if self.type == 'Trajectory':
return
elif self.type == 'Configs':
self.type = 'Trajectory'
else:
times = []
milestones = []
for i, s in enumerate(self.data.sections):
if i == 0:
if s.times is not None:
times += s.times
milestones += s.configs
else:
if s.times is not None:
times += s.times[1:]
milestones += s.configs[1:]
if len(times) == 0:
times = list(range(len(milestones)))
else:
assert len(times) == len(milestones)
self.data = Trajectory(times, milestones)
self.type = 'Trajectory'
def toConfigs(self):
if self.type == 'Trajectory':
self.type = 'Configs'
elif self.type == 'Configs':
return
else:
self.toTrajectory()
self.data.times = list(range(len(self.data.milestones)))
self.type = 'Configs'
def concat(self, other):
"""Warning: destructive"""
if self.type == 'MultiPath':
other.toMultipath()
elif other.type == 'MultiPath':
self.toMultipath()
if self.type == 'MultiPath':
self.data.concat(other.data)
else:
self.data.concat(other.data, relative=True)
def discretize(self, dt):
if self.type == 'MultiPath':
print("Warning, MultiPath discretize is not supported yet")
else:
self.data = self.data.discretize(dt)
def __str__(self):
if self.type == 'MultiPath':
ndofs = 0
if len(self.data.sections) > 0:
ndofs = len(self.data.sections[0].configs[0])
res = "MultiPath with %d DOFs, %d sections\n" % (
ndofs, len(self.data.sections))
sstrs = []
for i, s in enumerate(self.data.sections):
sres = " Section %d: %d milestones" % (i, len(s.configs))
if s.times != None:
sres += " / times"
if s.velocities != None:
sres += " / velocities"
sres += ", " + str(len(self.data.getStance(i))) + " holds"
sstrs.append(sres)
return res + '\n'.join(sstrs)
else:
ndofs = 0
if len(self.data.milestones) > 0:
ndofs = len(self.data.milestones[0])
return '%s with %d DOFs, %d milestones' % (
self.type, ndofs, len(self.data.milestones))