def edit(name,
value,
type='auto',
description=None,
editor='visual',
world=None,
frame=None):
"""Launches an editor for the given value. Returns a pair (save,result)
where save indicates what the user wanted to do with the edited value
and result is the edited value."""
if name == None and type == 'auto':
raise RuntimeError(
"Cannot do an anonymous edit without the 'type' argument specified"
)
if name == None:
name = 'Anonymous'
if type == 'auto':
type = nameToType(name)
if not _PyQtAvailable and editor == 'visual':
print "PyQt is not available, defaulting to console editor"
editor = 'console'
if isinstance(world, str):
#a single argument, e.g., a robot file
global _editTemporaryWorlds
if world not in _editTemporaryWorlds:
_editTemporaryWorlds[world] = WorldModel()
if not _editTemporaryWorlds[world].readFile(world):
raise RuntimeError("Error loading world file " + world)
world = _editTemporaryWorlds[world]
if isinstance(frame, str):
try:
oframe = world.rigidObject(frame)
frame = oframe
except RuntimeError:
try:
oframe = world.robot(0).getLink(frame)
frame = oframe
except RuntimeError:
try:
oframe = world.terrain(frame)
frame = oframe
except RuntimeError:
raise RuntimeError('Named frame "' + frame +
'" is not a valid frame')
if value == None:
if type == 'Config':
if world == None:
raise RuntimeError(
"Cannot visually edit a Config resource without a world")
value = world.robot(0).getConfig()
elif type == 'Configs':
raise RuntimeError(
"Cannot visually edit a Configs resource without a world")
value = [world.robot(0).getConfig()]
elif type == 'IKGoal':
value = IKObjective()
elif type == 'Vector3' or type == 'Point':
value = [0, 0, 0]
elif type == 'Rotation':
value = so3.identity()
elif type == 'RigidTransform':
value = se3.identity()
else:
raise RuntimeError("Don't know how to edit objects of type " +
type)
if editor == 'console':
return console_edit(name, value, type, description, world, frame)
elif editor == 'visual':
if type == 'Config':
return _launch(_ConfigVisualEditor(name, value, description,
world))
elif type == 'Configs':
return _launch(
_ConfigsVisualEditor(name, value, description, world))
elif type == 'Vector3' or type == 'Point':
if isinstance(frame, (RigidObjectModel, RobotModelLink)):
frame = frame.getTransform()
return _launch(
_PointVisualEditor(name, value, description, world, frame))
elif type == 'Rotation':
if isinstance(frame, (RigidObjectModel, RobotModelLink)):
frame = frame.getTransform()
return _launch(
_RotationVisualEditor(name, value, description, world, frame))
elif type == 'RigidTransform':
if isinstance(frame, RigidObjectModel):
return _launch(
_ObjectTransformVisualEditor(name, value, description,
world, frame))
if isinstance(frame, RobotModelLink):
frame = frame.getTransform()
return _launch(
_RigidTransformVisualEditor(name, value, description, world,
frame))
else:
raise RuntimeError("Don't know how to edit objects of type " +
type)
else:
raise ValueError(
"Invalid value for argument 'editor', must be either 'visual' or 'console'"
)
def __init__(self):
Context.__init__(self)
self.type = Type('V',9)
Rvar = Variable("R",self.type)
Rsymb = VariableExpression(Rvar)
R1 = Variable("R1",self.type)
R2 = Variable("R2",self.type)
V3type = Type('V',3)
q = Variable('q',Type('V',4))
pointvar = Variable("point",V3type)
pointsymb = VariableExpression(pointvar)
self.identity = self.declare(expr(so3.identity()),"identity",[])
self.identity.description = "The identity rotation"
self.matrix = self.declare(expr(so3.matrix(Rsymb)),"matrix",["R"])
self.matrix.addSimplifier(['so3.identity'],(lambda R:eye(3)),pre=True)
self.matrix.description = "Converts to a 3x3 matrix"
M = Variable("M",Type('M',(3,3)))
self.from_matrix = self.declare(flatten(transpose(M)),"from_matrix",['M'])
self.from_matrix.description = "Converts from a 3x3 matrix"
self.from_matrix.autoSetJacobians()
self.inv = self.declare(expr(so3.inv(Rsymb)),"inv",["R"])
self.inv.description = "Inverts a rotation"
self.inv.autoSetJacobians()
self.inv.properties['inverse'] = weakref.proxy(self.inv)
self.inv.addSimplifier(['so3.identity'],lambda R:R)
self.mul = self.declare(so3.mul,"mul")
self.mul.description = "Inverts a rotation"
self.mul.setDeriv(0,lambda R1,R2,dR1:self.mul(dR1,R2),asExpr=True)
self.mul.setDeriv(1,lambda R1,R2,dR2:self.mul(R1,dR2),asExpr=True)
self.mul.addSimplifier(['so3.identity',None],(lambda R1,R2:R2),pre=True)
self.mul.addSimplifier([None,'so3.identity'],(lambda R1,R2:R1),pre=True)
self.mul.properties['associative'] = True
self.apply = self.declare(expr(so3.apply(Rsymb,pointsymb)),"apply",["R","point"])
self.apply.addSimplifier(['so3.identity',None],(lambda R,point:point),pre=True)
self.apply.addSimplifier([None,'zero'],(lambda R,point:point),pre=True)
self.apply.autoSetJacobians()
self.rotation = self.declare(so3.rotation,"rotation")
self.from_rpy = self.declare(so3.from_rpy,"from_rpy")
self.rpy = self.declare(so3.rpy,"rpy")
self.from_quaternion = self.declare(expr(so3.from_quaternion([q[0],q[1],q[2],q[3]])),"from_quaternion",["q"])
self.quaternion = self.declare(so3.quaternion,"quaternion")
self.from_rotation_vector = self.declare(so3.from_rotation_vector,"from_rotation_vector")
self.rotation_vector = self.declare(so3.rotation_vector,"rotation_vector")
self.axis = self.declare(unit(self.rotation_vector(Rvar)),"rotation",["R"])
self.angle = self.declare(so3.angle,"angle")
self.error = self.declare(so3.error,"error")
self.distance = self.declare(self.angle(self.mul(self.inv(R1),R2)),"distance",['R1','R2'])
self.distance.properties['nonnegative'] = True
Rm = self.matrix(Rsymb)
self.eq_constraint = self.declare(dot(Rm.T,Rm),'eq_constraint',['R'])
self.quaternion_constraint = self.declare(norm2(q)-1,'quaternion_constraint',['q'])
self.identity.returnType = self.type
self.inv.returnType = self.type
self.inv.argTypes = [self.type]
self.mul.returnType = self.type
self.mul.argTypes = [self.type,self.type]
self.apply.returnType = V3type
self.apply.argTypes = [self.type,V3type]
self.rotation.returnType = self.type
self.rotation.argTypes = [V3type,Numeric]
self.rotation.setDeriv(1,lambda axis,angle:so3.cross_product(axis))
self.axis.returnType = V3type
self.axis.argTypes = [self.type]
self.angle.returnType = V3type
self.angle.argTypes = [self.type]
def angle_deriv(R,dR):
cosangle = (R[0]+R[4]+R[8]-1)*0.5
angle = arccos(cosangle)
#dangle / dR[0] = -1.0/sqrt(1-cosangle**2) * dcosangle/dR[0]
dacos = -1.0/sqrt(1-cosangle**2)
return expr([0.5*dacos*dR[0],0,0,0,0.5*dacos*dR[4],0,0,0,0.5*dacos*dR[8]])
self.angle.setDeriv(0,angle_deriv,asExpr=True)
self.error.returnType = V3type
self.error.argTypes = [self.type,self.type]
self.distance.returnType = Numeric
self.distance.argTypes = [self.type,self.type]
self.distance.autoSetJacobians()
self.from_matrix.returnType = self.type
self.from_matrix.argTypes = [M.type]
self.from_rpy.returnType = self.type
self.from_rpy.argTypes = [V3type]
self.from_quaternion.returnType = self.type
self.from_quaternion.argTypes = [Type('V',4)]
self.from_rotation_vector.returnType = self.type
self.from_rotation_vector.argTypes = [V3type]
self.matrix.returnType = self.from_matrix.argTypes[0]
self.matrix.argTypes = [self.from_matrix.returnType]
self.rpy.returnType = self.from_rpy.argTypes[0]
self.rpy.argTypes = [self.from_rpy.returnType]
self.quaternion.returnType = self.from_quaternion.argTypes[0]
self.quaternion.argTypes = [self.from_quaternion.returnType]
self.rotation_vector.returnType = self.from_rotation_vector.argTypes[0]
self.rotation_vector.argTypes = [self.from_rotation_vector.returnType]
def draw(self,world=None):
"""Draws the specified item in the specified world. If name
is given and text_hidden != False, then the name of the item is
shown."""
if self.hidden: return
item = self.item
name = self.name
#set appearance
if not self.useDefaultAppearance and hasattr(item,'appearance'):
if not hasattr(self,'oldAppearance'):
self.oldAppearance = item.appearance().clone()
if self.customAppearance != None:
print "Changing appearance of",name
item.appearance().set(self.customAppearance)
elif "color" in self.attributes:
print "Changing color of",name
item.appearance().setColor(*self.attributes["color"])
if hasattr(item,'drawGL'):
item.drawGL()
elif len(self.subAppearances)!=0:
for n,app in self.subAppearances.iteritems():
app.widget = self.widget
app.draw(world)
elif isinstance(item,coordinates.Point):
def drawRaw():
glDisable(GL_DEPTH_TEST)
glDisable(GL_LIGHTING)
glEnable(GL_POINT_SMOOTH)
glPointSize(self.attributes.get("size",5.0))
glColor4f(*self.attributes.get("color",[0,0,0,1]))
glBegin(GL_POINTS)
glVertex3f(0,0,0)
glEnd()
glEnable(GL_DEPTH_TEST)
#write name
self.displayCache[0].draw(drawRaw,[so3.identity(),item.worldCoordinates()])
if name != None:
self.drawText(name,vectorops.add(item.worldCoordinates(),[0,0,-0.05]))
elif isinstance(item,coordinates.Direction):
def drawRaw():
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
L = self.attributes.get("length",0.15)
source = [0,0,0]
glColor4f(*self.attributes.get("color",[0,1,1,1]))
glBegin(GL_LINES)
glVertex3f(*source)
glVertex3f(*vectorops.mul(item.localCoordinates(),L))
glEnd()
glEnable(GL_DEPTH_TEST)
#write name
self.displayCache[0].draw(drawRaw,item.frame().worldCoordinates(),parameters = item.localCoordinates())
if name != None:
self.drawText(name,vectorops.add(vectorops.add(item.frame().worldCoordinates()[1],item.worldCoordinates()),[0,0,-0.05]))
elif isinstance(item,coordinates.Frame):
t = item.worldCoordinates()
if item.parent() != None:
tp = item.parent().worldCoordinates()
else:
tp = se3.identity()
tlocal = item.relativeCoordinates()
def drawRaw():
glDisable(GL_DEPTH_TEST)
glDisable(GL_LIGHTING)
glLineWidth(2.0)
gldraw.xform_widget(tlocal,self.attributes.get("length",0.1),self.attributes.get("width",0.01))
glLineWidth(1.0)
#draw curve between frame and parent
if item.parent() != None:
d = vectorops.norm(tlocal[1])
vlen = d*0.5
v1 = so3.apply(tlocal[0],[-vlen]*3)
v2 = [vlen]*3
#glEnable(GL_BLEND)
#glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
#glColor4f(1,1,0,0.5)
glColor3f(1,1,0)
gldraw.hermite_curve(tlocal[1],v1,[0,0,0],v2,0.03)
#glDisable(GL_BLEND)
glEnable(GL_DEPTH_TEST)
#For some reason, cached drawing is causing OpenGL problems
#when the frame is rapidly changing
#self.displayCache[0].draw(drawRaw,transform=tp, parameters = tlocal)
glPushMatrix()
glMultMatrixf(sum(zip(*se3.homogeneous(tp)),()))
drawRaw()
glPopMatrix()
#write name
if name != None:
self.drawText(name,se3.apply(t,[-0.05]*3))
elif isinstance(item,coordinates.Transform):
#draw curve between frames
t1 = item.source().worldCoordinates()
if item.destination() != None:
t2 = item.destination().worldCoordinates()
else:
t2 = se3.identity()
d = vectorops.distance(t1[1],t2[1])
vlen = d*0.5
v1 = so3.apply(t1[0],[-vlen]*3)
v2 = so3.apply(t2[0],[vlen]*3)
def drawRaw():
glDisable(GL_DEPTH_TEST)
glDisable(GL_LIGHTING)
glColor3f(1,1,1)
gldraw.hermite_curve(t1[1],v1,t2[1],v2,0.03)
glEnable(GL_DEPTH_TEST)
#write name at curve
self.displayCache[0].draw(drawRaw,transform=None,parameters = (t1,t2))
if name != None:
self.drawText(name,spline.hermite_eval(t1[1],v1,t2[1],v2,0.5))
else:
types = resource.objectToTypes(item,world)
if isinstance(types,(list,tuple)):
#ambiguous, still need to figure out what to draw
validtypes = []
for t in types:
if t == 'Config':
if world != None and len(t) == world.robot(0).numLinks():
validtypes.append(t)
elif t=='Vector3':
validtypes.append(t)
elif t=='RigidTransform':
validtypes.append(t)
if len(validtypes) > 1:
print "Unable to draw item of ambiguous types",validtypes
return
if len(validtypes) == 0:
print "Unable to draw any of types",types
return
types = validtypes[0]
if types == 'Config':
if world:
robot = world.robot(0)
if not self.useDefaultAppearance:
oldAppearance = [robot.link(i).appearance().clone() for i in xrange(robot.numLinks())]
for i in xrange(robot.numLinks()):
robot.link(i).appearance().set(self.customAppearance)
oldconfig = robot.getConfig()
robot.setConfig(item)
robot.drawGL()
robot.setConfig(oldconfig)
if not self.useDefaultAppearance:
for (i,app) in enumerate(oldAppearance):
robot.link(i).appearance().set(app)
else:
print "Unable to draw Config's without a world"
elif types == 'Vector3':
def drawRaw():
glDisable(GL_LIGHTING)
glEnable(GL_POINT_SMOOTH)
glPointSize(self.attributes.get("size",5.0))
glColor4f(*self.attributes.get("color",[0,0,0,1]))
glBegin(GL_POINTS)
glVertex3f(0,0,0)
glEnd()
self.displayCache[0].draw(drawRaw,[so3.identity(),item])
if name != None:
self.drawText(name,vectorops.add(item,[0,0,-0.05]))
elif types == 'RigidTransform':
def drawRaw():
gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01))
self.displayCache[0].draw(drawRaw,transform=item)
if name != None:
self.drawText(name,se3.apply(item,[-0.05]*3))
elif types == 'IKGoal':
if hasattr(item,'robot'):
#need this to be built with a robot element.
#Otherwise, can't determine the correct transforms
robot = item.robot
elif world:
if world.numRobots() >= 1:
robot = world.robot(0)
else:
robot = None
else:
robot = None
if robot != None:
link = robot.link(item.link())
dest = robot.link(item.destLink()) if item.destLink()>=0 else None
while len(self.displayCache) < 3:
self.displayCache.append(CachedGLObject())
self.displayCache[1].name = self.name+" target position"
self.displayCache[2].name = self.name+" curve"
if item.numPosDims() != 0:
lp,wp = item.getPosition()
#set up parameters of connector
p1 = se3.apply(link.getTransform(),lp)
if dest != None:
p2 = se3.apply(dest.getTransform(),wp)
else:
p2 = wp
d = vectorops.distance(p1,p2)
v1 = [0.0]*3
v2 = [0.0]*3
if item.numRotDims()==3: #full constraint
R = item.getRotation()
def drawRaw():
gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01))
t1 = se3.mul(link.getTransform(),(so3.identity(),lp))
t2 = (R,wp) if dest==None else se3.mul(dest.getTransform(),(R,wp))
self.displayCache[0].draw(drawRaw,transform=t1)
self.displayCache[1].draw(drawRaw,transform=t2)
vlen = d*0.1
v1 = so3.apply(t1[0],[-vlen]*3)
v2 = so3.apply(t2[0],[vlen]*3)
elif item.numRotDims()==0: #point constraint
def drawRaw():
glDisable(GL_LIGHTING)
glEnable(GL_POINT_SMOOTH)
glPointSize(self.attributes.get("size",5.0))
glColor4f(*self.attributes.get("color",[0,0,0,1]))
glBegin(GL_POINTS)
glVertex3f(0,0,0)
glEnd()
self.displayCache[0].draw(drawRaw,transform=(so3.identity(),p1))
self.displayCache[1].draw(drawRaw,transform=(so3.identity(),p2))
#set up the connecting curve
vlen = d*0.5
d = vectorops.sub(p2,p1)
v1 = vectorops.mul(d,0.5)
#curve in the destination
v2 = vectorops.cross((0,0,0.5),d)
else: #hinge constraint
p = [0,0,0]
d = [0,0,0]
def drawRawLine():
glDisable(GL_LIGHTING)
glEnable(GL_POINT_SMOOTH)
glPointSize(self.attributes.get("size",5.0))
glColor4f(*self.attributes.get("color",[0,0,0,1]))
glBegin(GL_POINTS)
glVertex3f(*p)
glEnd()
glColor4f(*self.attributes.get("color",[0.5,0,0.5,1]))
glLineWidth(self.attributes.get("width",3.0))
glBegin(GL_LINES)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p,d,self.attributes.get("length",0.1)))
glEnd()
glLineWidth(1.0)
ld,wd = item.getRotationAxis()
p = lp
d = ld
self.displayCache[0].draw(drawRawLine,transform=link.getTransform(),parameters=(p,d))
p = wp
d = wd
self.displayCache[1].draw(drawRawLine,transform=dest.getTransform() if dest else se3.identity(),parameters=(p,d))
#set up the connecting curve
d = vectorops.sub(p2,p1)
v1 = vectorops.mul(d,0.5)
#curve in the destination
v2 = vectorops.cross((0,0,0.5),d)
def drawConnection():
glDisable(GL_DEPTH_TEST)
glDisable(GL_LIGHTING)
glColor3f(1,0.5,0)
gldraw.hermite_curve(p1,v1,p2,v2,0.03)
glEnable(GL_DEPTH_TEST)
self.displayCache[2].draw(drawConnection,transform=None,parameters = (p1,v1,p2,v2))
if name != None:
self.drawText(name,vectorops.add(wp,[-0.05]*3))
else:
wp = link.getTransform()[1]
if item.numRotDims()==3: #full constraint
R = item.getRotation()
def drawRaw():
gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01))
self.displayCache[0].draw(drawRaw,transform=link.getTransform())
self.displayCache[1].draw(drawRaw,transform=se3.mul(link.getTransform(),(R,[0,0,0])))
elif item.numRotDims() > 0:
#axis constraint
d = [0,0,0]
def drawRawLine():
glDisable(GL_LIGHTING)
glColor4f(*self.attributes.get("color",[0.5,0,0.5,1]))
glLineWidth(self.attributes.get("width",3.0))
glBegin(GL_LINES)
glVertex3f(0,0,0)
glVertex3f(*vectorops.mul(d,self.attributes.get("length",0.1)))
glEnd()
glLineWidth(1.0)
ld,wd = item.getRotationAxis()
d = ld
self.displayCache[0].draw(drawRawLine,transform=link.getTransform(),parameters=d)
d = wd
self.displayCache[1].draw(drawRawLine,transform=(dest.getTransform()[0] if dest else so3.identity(),wp),parameters=d)
else:
#no drawing
pass
if name != None:
self.drawText(name,se3.apply(wp,[-0.05]*3))
else:
print "Unable to draw item of type",types
#revert appearance
if not self.useDefaultAppearance and hasattr(item,'appearance'):
item.appearance().set(self.oldAppearance)
def __init__(self):
Context.__init__(self)
self.type = Type('V', 9)
Rvar = Variable("R", self.type)
Rsymb = VariableExpression(Rvar)
R1 = Variable("R1", self.type)
R2 = Variable("R2", self.type)
V3type = Type('V', 3)
q = Variable('q', Type('V', 4))
pointvar = Variable("point", V3type)
pointsymb = VariableExpression(pointvar)
self.identity = self.declare(expr(so3.identity()), "identity", [])
self.identity.description = "The identity rotation"
self.matrix = self.declare(expr(so3.matrix(Rsymb)), "matrix", ["R"])
self.matrix.addSimplifier(['so3.identity'], (lambda R: eye(3)),
pre=True)
self.matrix.description = "Converts to a 3x3 matrix"
M = Variable("M", Type('M', (3, 3)))
self.from_matrix = self.declare(flatten(transpose(M)), "from_matrix",
['M'])
self.from_matrix.description = "Converts from a 3x3 matrix"
self.from_matrix.autoSetJacobians()
self.inv = self.declare(expr(so3.inv(Rsymb)), "inv", ["R"])
self.inv.description = "Inverts a rotation"
self.inv.autoSetJacobians()
self.inv.properties['inverse'] = weakref.proxy(self.inv)
self.inv.addSimplifier(['so3.identity'], lambda R: R)
self.mul = self.declare(so3.mul, "mul")
self.mul.description = "Inverts a rotation"
self.mul.setDeriv(0,
lambda R1, R2, dR1: self.mul(dR1, R2),
asExpr=True)
self.mul.setDeriv(1,
lambda R1, R2, dR2: self.mul(R1, dR2),
asExpr=True)
self.mul.addSimplifier(['so3.identity', None], (lambda R1, R2: R2),
pre=True)
self.mul.addSimplifier([None, 'so3.identity'], (lambda R1, R2: R1),
pre=True)
self.mul.properties['associative'] = True
self.apply = self.declare(expr(so3.apply(Rsymb, pointsymb)), "apply",
["R", "point"])
self.apply.addSimplifier(['so3.identity', None],
(lambda R, point: point),
pre=True)
self.apply.addSimplifier([None, 'zero'], (lambda R, point: point),
pre=True)
self.apply.autoSetJacobians()
self.rotation = self.declare(so3.rotation, "rotation")
self.from_rpy = self.declare(so3.from_rpy, "from_rpy")
self.rpy = self.declare(so3.rpy, "rpy")
self.from_quaternion = self.declare(
expr(so3.from_quaternion([q[0], q[1], q[2], q[3]])),
"from_quaternion", ["q"])
self.quaternion = self.declare(so3.quaternion, "quaternion")
self.from_rotation_vector = self.declare(so3.from_rotation_vector,
"from_rotation_vector")
self.rotation_vector = self.declare(so3.rotation_vector,
"rotation_vector")
self.axis = self.declare(unit(self.rotation_vector(Rvar)), "rotation",
["R"])
self.angle = self.declare(so3.angle, "angle")
self.error = self.declare(so3.error, "error")
self.distance = self.declare(self.angle(self.mul(self.inv(R1), R2)),
"distance", ['R1', 'R2'])
self.distance.properties['nonnegative'] = True
Rm = self.matrix(Rsymb)
self.eq_constraint = self.declare(dot(Rm.T, Rm), 'eq_constraint',
['R'])
self.quaternion_constraint = self.declare(
norm2(q) - 1, 'quaternion_constraint', ['q'])
self.identity.returnType = self.type
self.inv.returnType = self.type
self.inv.argTypes = [self.type]
self.mul.returnType = self.type
self.mul.argTypes = [self.type, self.type]
self.apply.returnType = V3type
self.apply.argTypes = [self.type, V3type]
self.rotation.returnType = self.type
self.rotation.argTypes = [V3type, Numeric]
self.rotation.setDeriv(1, lambda axis, angle: so3.cross_product(axis))
self.axis.returnType = V3type
self.axis.argTypes = [self.type]
self.angle.returnType = V3type
self.angle.argTypes = [self.type]
def angle_deriv(R, dR):
cosangle = (R[0] + R[4] + R[8] - 1) * 0.5
angle = arccos(cosangle)
#dangle / dR[0] = -1.0/sqrt(1-cosangle**2) * dcosangle/dR[0]
dacos = -1.0 / sqrt(1 - cosangle**2)
return expr([
0.5 * dacos * dR[0], 0, 0, 0, 0.5 * dacos * dR[4], 0, 0, 0,
0.5 * dacos * dR[8]
])
self.angle.setDeriv(0, angle_deriv, asExpr=True)
self.error.returnType = V3type
self.error.argTypes = [self.type, self.type]
self.distance.returnType = Numeric
self.distance.argTypes = [self.type, self.type]
self.distance.autoSetJacobians()
self.from_matrix.returnType = self.type
self.from_matrix.argTypes = [M.type]
self.from_rpy.returnType = self.type
self.from_rpy.argTypes = [V3type]
self.from_quaternion.returnType = self.type
self.from_quaternion.argTypes = [Type('V', 4)]
self.from_rotation_vector.returnType = self.type
self.from_rotation_vector.argTypes = [V3type]
self.matrix.returnType = self.from_matrix.argTypes[0]
self.matrix.argTypes = [self.from_matrix.returnType]
self.rpy.returnType = self.from_rpy.argTypes[0]
self.rpy.argTypes = [self.from_rpy.returnType]
self.quaternion.returnType = self.from_quaternion.argTypes[0]
self.quaternion.argTypes = [self.from_quaternion.returnType]
self.rotation_vector.returnType = self.from_rotation_vector.argTypes[0]
self.rotation_vector.argTypes = [self.from_rotation_vector.returnType]
def edit(name,value,type='auto',description=None,editor='visual',world=None,frame=None):
"""Launches an editor for the given value. Returns a pair (save,result)
where save indicates what the user wanted to do with the edited value
and result is the edited value."""
if name == None and type=='auto':
raise RuntimeError("Cannot do an anonymous edit without the 'type' argument specified")
if name == None:
name = 'Anonymous'
if type == 'auto':
type = nameToType(name)
if not _PyQtAvailable and editor=='visual':
print "PyQt is not available, defaulting to console editor"
editor = 'console'
if isinstance(world,str):
#a single argument, e.g., a robot file
global _editTemporaryWorlds
if world not in _editTemporaryWorlds:
_editTemporaryWorlds[world] = WorldModel()
if not _editTemporaryWorlds[world].readFile(world):
raise RuntimeError("Error loading world file "+world)
world = _editTemporaryWorlds[world]
if isinstance(frame,str):
try:
oframe = world.rigidObject(frame)
frame = oframe
except RuntimeError:
try:
oframe = world.robot(0).getLink(frame)
frame = oframe
except RuntimeError:
try:
oframe = world.terrain(frame)
frame = oframe
except RuntimeError:
raise RuntimeError('Named frame "'+frame+'" is not a valid frame')
if value==None:
if type == 'Config':
if world==None:
raise RuntimeError("Cannot visually edit a Config resource without a world")
value = world.robot(0).getConfig()
elif type == 'Configs':
raise RuntimeError("Cannot visually edit a Configs resource without a world")
value = [world.robot(0).getConfig()]
elif type == 'IKGoal':
value = IKObjective()
elif type == 'Vector3' or type == 'Point':
value = [0,0,0]
elif type == 'Rotation':
value = so3.identity()
elif type == 'RigidTransform':
value = se3.identity()
else:
raise RuntimeError("Don't know how to edit objects of type "+type)
if editor == 'console':
return console_edit(name,value,type,description,world,frame)
elif editor == 'visual':
if type == 'Config':
return _launch(_ConfigVisualEditor(name,value,description,world))
elif type == 'Configs':
return _launch(_ConfigsVisualEditor(name,value,description,world))
elif type == 'Vector3' or type == 'Point':
if isinstance(frame,(RigidObjectModel,RobotModelLink)):
frame = frame.getTransform()
return _launch(_PointVisualEditor(name,value,description,world,frame))
elif type == 'Rotation':
if isinstance(frame,(RigidObjectModel,RobotModelLink)):
frame = frame.getTransform()
return _launch(_RotationVisualEditor(name,value,description,world,frame))
elif type == 'RigidTransform':
if isinstance(frame,RigidObjectModel):
return _launch(_ObjectTransformVisualEditor(name,value,description,world,frame))
if isinstance(frame,RobotModelLink):
frame = frame.getTransform()
return _launch(_RigidTransformVisualEditor(name,value,description,world,frame))
else:
raise RuntimeError("Don't know how to edit objects of type "+type)
else:
raise ValueError("Invalid value for argument 'editor', must be either 'visual' or 'console'")
def from_translation(t):
"""Returns a transformation T that translates points by t"""
return (so3.identity(), t[:])
def from_translation(t):
"""Returns a transformation T that translates points by t"""
return (so3.identity(),t[:])