def test_no_evalf(self):
'''Variables and Parameters do not evaluate to floats'''
v = values.Variable('v')
p = values.Parameter(name='p',
defValue=0.0) # even if it has a default value
e = values.Expression(v)
self.assertRaises(RuntimeError, e.evalf)
e = values.Expression(p)
self.assertRaises(RuntimeError, e.evalf)
def test_pi_expr_value(self):
'''Test numerical evaluation of a PI expression'''
coeff = random.random()
pi = values.MyPI.instance()
e = values.Expression(pi, pi.symbol * coeff)
err = round(e.evalf() - numpy.pi * coeff, 6)
self.assertTrue(err == 0.0)
def test_constant_value(self):
value = random.random()
c = values.Constant(name="c", value=value)
self.assertTrue(c.value == value)
e = values.Expression(c)
self.assertTrue(e.evalf() == value)
def symbolsGenerator():
# Generates a number 50% of the time, a symbol 50% of the time
if random.random() > 0.5:
return random.random()
else:
return numeric_argument.Expression(
numeric_argument.Variable(
name=random.choice(string.ascii_letters)))
def _getPropertyValue(yamlvalue, floatLiteralsAsConstants=False):
ret = yamlvalue
if isinstance(yamlvalue, numbers.Real) :
if floatLiteralsAsConstants :
cc = expr.Constant(name="", value=yamlvalue) #TODO name?!?
ret = expr.Expression(cc)
elif isinstance(yamlvalue, str) :
match = pimatcher.fullmatch(yamlvalue)
if not match :
raise RuntimeError("Unrecognized expression: '{0}'".format(yamlvalue))
else :
pi = expr.MyPI.instance()
# Make sure to replace the case-insensitive pi string with lowercase "pi"
exprtext = yamlvalue.replace( match.group(1), "pi" )
ret = expr.Expression(argument=pi, sympyExpr=sympy.sympify(exprtext))
else :
raise RuntimeError("Unknown value '{0}'".format(str(yamlvalue)))
return ret
def test_expression_attributes(self):
'''Test consistency of the attributes of a `kgprim.values.Expression`'''
name = "v1"
v1 = values.Variable(name)
e1 = values.Expression(v1)
# construct a non-trivial expression of v1
e2 = 3 * e1
s1 = sympy.Symbol(name)
# Check that the argument of the new expression is still the same
# Variable, and then check the Sympy attributes
self.assertTrue(e2.argument == v1)
self.assertTrue(e2.argument.symbol == s1)
self.assertTrue(e2.expr == 3 * s1)
def __init__(self, ctPrimitive, coordinateTransform):
if not isinstance(ctPrimitive.amount, numeric_argument.Expression):
raise RuntimeError(
'Need to pass a transform with an Expression as argument')
original = ctPrimitive.amount
# Sympy specifics here... might not be very robust...
# Essentially we want to isolate the same Expression but without any
# '-' in front
# We rely on the fact that the sympy epressions coming from an input
# geometry model are not more complicated than 'coefficient * symbol'
(mult, rest) = original.expr.as_coeff_mul()
sympynew = abs(mult) * rest[
0] # [0] here, assumption is that there is only one term other than the multiplier
expression = numeric_argument.Expression(argument=original.arg,
sympyExpr=sympynew)
self.expression = expression
self.rotation = (ctPrimitive.kind == MotionStep.Kind.Rotation)
def __raise(name):
raise RuntimeError(
"Cannot get the numeric value of parameter '{0}'".format(name))
__currentParams = {}
exprvalue_float = {
'PlainExpr': lambda e: (-1 if e.minus else 1) * e.arg,
'MultExpr': lambda e: e.mult * e.arg,
'DivExpr': lambda e: (-1 if e.minus else 1) * e.arg / e.div
}
exprvalue_symb = {
'PlainExpr':
lambda e: vpc.Expression(e.arg, (-1 if e.minus else 1) * e.arg.symbol),
'MultExpr':
lambda e: vpc.Expression(e.arg, e.mult * e.arg.symbol),
'DivExpr':
lambda e: vpc.Expression(e.arg, (-1
if e.minus else 1) * e.arg.symbol / e.div)
}
def exprValue(expr):
cname = expr.__class__.__name__
if cname not in exprvalue_float:
raise RuntimeError(
"Unsupported property type ({0}) in the input KinDSL model".format(
cname))
if isinstance(expr.arg, float):
def __init__(self, robot, frames, axes):
self.poseSpecByJoint = {}
self.poseSpecByPose = {}
self.jointToSymVar = {}
self.symVarToJoint = {}
for joint in robot.joints.values():
i = robot.jointNum(joint)
succ = robot.successor(joint)
succFrame = frames.linkFrames[succ]
jointFrame = frames.jointFrames[joint]
pose = Pose(target=succFrame, reference=jointFrame)
axis = axes[joint.name]
# So, we have the pose of the link (successor) frame relative to the joint frame
symname = "q{0}".format(i - 1)
qvar = numeric_argument.Variable(name=symname)
qexpr = numeric_argument.Expression(argument=qvar)
motionSteps = None
motionF = None
if joint.kind == 'fixed':
# aribtrary motion step of 0 value
motionSteps = [motions.translation(motions.Axis.Z, 0.0)]
else:
if joint.kind == JointKind.prismatic:
motionF = lambda axis, amount: motions.translation(
axis, amount)
elif joint.kind == JointKind.revolute:
motionF = lambda axis, amount: motions.rotation(
axis, amount)
else:
raise RuntimeError("Unknown joint type '{0}'".format(
joint.kind))
if axis == (1, 0, 0):
motionSteps = [motionF(motions.Axis.X, qexpr)]
elif axis == (-1, 0, 0):
motionSteps = [motionF(motions.Axis.X, -qexpr)]
elif axis == (0, 1, 0):
motionSteps = [motionF(motions.Axis.Y, qexpr)]
elif axis == (0, -1, 0):
motionSteps = [motionF(motions.Axis.Y, -qexpr)]
elif axis == (0, 0, 1):
motionSteps = [motionF(motions.Axis.Z, qexpr)]
elif axis == (0, 0, -1):
motionSteps = [motionF(motions.Axis.Z, -qexpr)]
else:
jointMotion = motionF(motions.Axis.Z, qexpr)
# The joint axis is an arbitrary versor...
# Thus, we first rotate the frame so that our Z axis aligns
# with the joint axis, and only then we add the motion of
# the joint itself.
x, y, z = axis[:]
if y != 0.0:
rz = -math.tan(x / y)
rx = -(math.pi / 2 -
math.tan(z / math.sqrt(x * x + y * y)))
step1 = motions.rotation(motions.Axis.Z, rz)
step2 = motions.rotation(motions.Axis.X, rx)
motionSteps = [
step1, step2, jointMotion, -step2, -step1
]
else:
ry = math.tan(
x / z
) # if z was also 0, another branch above would be executed
step1 = motions.rotation(motions.Axis.Y, ry)
motionSteps = [step1, jointMotion, -step1]
poseSpec = PoseSpec(
pose=pose,
motion=motions.MotionSequence(
steps=motionSteps,
mode=motions.MotionSequence.Mode.currentFrame))
self.poseSpecByJoint[joint] = poseSpec
self.poseSpecByPose[pose] = poseSpec
self.jointToSymVar[joint] = qvar
self.symVarToJoint[qvar] = joint
self.robot = robot
self.robotFrames = frames
self.jointPosesModel = motions.PosesSpec(
name=robot.name + '-joints', poses=self.poseSpecByJoint.values())