def test_poseInference(self):
mot1 = self.backend.randomMotion()
mot2 = self.backend.randomMotion()
pose1 = motions.PoseSpec(pose=pBA, motion=mot1)
pose2 = motions.PoseSpec(pose=pCB, motion=mot2)
model = motions.PosesSpec(name='test', poses=[pose1, pose2])
inspector = motions.ConnectedFramesInspector(model)
self.assertTrue(inspector.hasRelativePose(frA, frC))
posespec = inspector.getPoseSpec(targetFrame=frC, referenceFrame=frA)
A_ct_B = toCoordinateTransform(pose1)
B_ct_C = toCoordinateTransform(pose2)
A_ct_C = toCoordinateTransform(posespec) # to be tested
A_X_B = self.backend.asMatrix(A_ct_B)
B_X_C = self.backend.asMatrix(B_ct_C)
A_X_C = self.backend.asMatrix(A_ct_C)
self.assertTrue(
self.backend.equal_matrix(self.backend.mult_matrix(A_X_B, B_X_C),
A_X_C))
def convert(kindslFile, params={}):
global __currentParams
__currentParams = params
robin = dsl.modelFromFile(kindslFile)
links = ODict()
joints = ODict()
kpairs = []
allBodies = [robin.base] + robin.links
for linkin in allBodies:
name = linkin.name
link = robmodel.connectivity.Link(name)
links[name] = link
for jointin in robin.joints:
name = jointin.name
joint = robmodel.connectivity.Joint(
name, jointTypeStr[jointin.__class__.__name__])
joints[name] = joint
for parentin in allBodies:
for childSpec in parentin.childrenList.children:
childin = childSpec.link
jointin = childSpec.joint
parent = links[parentin.name]
child = links[childin.name]
joint = joints[jointin.name]
kpairs.append(robmodel.connectivity.KPair(joint, parent, child))
connectivityModel = robmodel.connectivity.Robot(robin.name, links, joints,
kpairs)
# NUMBERING SCHEME
numbering = {}
numbering[robin.base.name] = 0
for linkin in robin.links:
numbering[linkin.name] = linkin.num
ordering = {'robot': robin.name, 'nums': numbering}
orderedModel = robmodel.ordering.Robot(connectivityModel, ordering)
# FRAMES and POSES
userFrames = []
for linkin in allBodies:
for fr in linkin.frames:
frame = primitives.Frame(fr.name)
attach = primitives.Attachment(entity=frame,
body=links[linkin.name])
userFrames.append(attach)
framesModel = robmodel.frames.RobotDefaultFrames(orderedModel, userFrames)
poses = []
for jointin in robin.joints:
motion_link_to_joint = linkFrameToOtherFrameInKinDSL(jointin.refFrame)
joint = joints[jointin.name]
link = orderedModel.predecessor(joint)
frame_joint = framesModel.framesByName[joint.name]
frame_link = framesModel.framesByName[link.name]
pose = primitives.Pose(target=frame_joint, reference=frame_link)
poses.append(PoseSpec(pose, motion_link_to_joint))
for linkin in allBodies:
for fr in linkin.frames:
motion_link_to_user = linkFrameToOtherFrameInKinDSL(fr.transform)
frame_link = framesModel.framesByName[linkin.name]
frame_user = framesModel.framesByName[fr.name]
pose = primitives.Pose(target=frame_user, reference=frame_link)
poses.append(PoseSpec(pose, motion_link_to_user))
posesModel = motions.PosesSpec(robin.name, poses)
geometryModel = robmodel.geometry.Geometry(orderedModel, framesModel,
posesModel)
inertiadict = {}
inertiaBodies = robin.links
if robin.base.__class__.__name__ == 'FloatingRobotBase':
inertiaBodies = allBodies
for linkin in inertiaBodies:
ipin = linkin.bodyInertia
#TODO support the inertia properties in the user-frame
frame = framesModel.framesByName[linkin.name]
mass = exprValue(ipin.mass)
com = inertia.CoM(frame,
x=exprValue(ipin.com.x),
y=exprValue(ipin.com.y),
z=exprValue(ipin.com.z))
im = inertia.IMoments(frame,
ixx=exprValue(ipin.ixx),
iyy=exprValue(ipin.iyy),
izz=exprValue(ipin.izz),
ixy=exprValue(ipin.ixy),
ixz=exprValue(ipin.ixz),
iyz=exprValue(ipin.iyz))
inertiadict[linkin.name] = inertia.BodyInertia(mass, com, im)
inertiaModel = inertia.RobotLinksInertia(connectivityModel, framesModel,
inertiadict)
return connectivityModel, orderedModel, framesModel, geometryModel, inertiaModel
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())
def __init__(self, connectModel, framesModel, posesModel, jointAxes=None):
'''
Construct the geometry model of a mechanism by composing the arguments.
Parameters:
- `connectModel` the connectivity model of the mechanism, with ordering
- `framesModel` the set of the Cartesian frames attached to the mechanism
- `posesModel` the constant, relative poses between the frames
- `jointAxes` the versors of the joint axes, in joint frame coordinates
The third argument is the actual geometric data; this constructor makes
sure that the three arguments are consistent and therefore can be
composed as a whole.
In fact, the poses model stored in this instance is not the given
`posesModel`. The input poses are always replaced by poses pointing to
objects of `framesModel`. This is because the frames in the given
`posesModel` might be simple placeholders. The name of the frame is used
to establish the correspondence between a placeholder frame and a
robot-attached frame.
The `jointAxes` argument defaults to `None`. In that case it is assumed
that the axis of any joint is the Z axis of its frame. Otherwise, the
argument should be a dictionary indexed by joint name, with values being
3-value tuples. Any tuple must represent the 3D joint axis versor, using
coordinates in the joint frame.
'''
if not isinstance(connectModel, ordering.Robot):
raise RuntimeError("An ordered robot model is required")
rname = connectModel.name
if framesModel.robot.name != rname:
raise RuntimeError(
"Robot name mismatch in the connectivity and frames model ('{0}' vs '{1})"
.format(rname, framesModel.robot.name))
self.byPose = {}
for poseSpec in posesModel.poses:
ignore = False
pose = poseSpec.pose
warnmsg = '''Frame '{0}' not found on the given frames-model '{1}', ignoring'''
if pose.target.name not in framesModel.framesByName:
logger.warning(warnmsg.format(pose.target.name, rname))
ignore = True
if pose.reference.name not in framesModel.framesByName:
logger.warning(warnmsg.format(pose.reference.name, rname))
ignore = True
if not ignore:
tgtF = framesModel.framesByName[pose.target.name]
refF = framesModel.framesByName[pose.reference.name]
path = framesModel.path(tgtF, refF)
for i in range(len(path) - 1):
kind = framesModel.kind(path[i], path[i + 1])
if kind == frames.FrameRelationKind.acrossJoint:
warnmsg = '''Found non-constant pose in the geometry model
('{0}' with respect to '{1}')'''.format(
tgtF.name, refF.name)
logger.warning(warnmsg)
# Rebuild the Pose instance with the frames from the frames
# model, which are attached to links. The pose instance from the
# motions model (posesModel) might only refer to named frames
realpose = Pose(target=tgtF, reference=refF)
realposespec = motions.PoseSpec(pose=realpose,
motion=poseSpec.motion)
self.byPose[realpose] = realposespec
#print( realpose )
#print( poseSpec.motion.steps, "\n\n")
# We iterate over joints and fetch the predecessor, as the joint_wrt_predecessor
# is a constant pose also for loop joints.
self.byJoint = {}
for joint in connectModel.joints.values():
predFrame = framesModel.linkFrames[connectModel.predecessor(joint)]
jointFrame = framesModel.jointFrames[joint]
pose = Pose(target=jointFrame, reference=predFrame)
if pose not in self.byPose:
logger.warning(
"The geometry model does not seem to have information about the pose of '{0}' wrt '{1}'"
.format(jointFrame.name, predFrame.name))
else:
self.byJoint[joint] = self.byPose[pose]
self.ordering = connectModel
self.frames = framesModel
self.poses = motions.PosesSpec(posesModel.name,
list(self.byPose.values()))
# The last line creates a new PosesSpec model, to make sure we store the
# model whose poses reference robot attached frames. As said before, the
# PosesSpec model given to the constructor might have poses that refer
# to the un-attached frames.
if jointAxes == None:
jointAxes = {}
for joint in connectModel.joints.values():
jointAxes[joint.name] = (0.0, 0.0, 1.0) # default is Z axis
else:
if jointAxes.keys() != connectModel.joints.keys():
logger.warning(
"The names in the joint-axes dictionary do not " +
"match the names in the connectivity model")
self.axes = jointAxes
def convert( urdf ) :
'''
Reads the model from a URDFWrapper instance, and construct the corresponding
models in our format.
'''
robotName = urdf.robotName
links = ODict()
joints = ODict()
pairs = []
children = {}
orphans = []
for urdfname in urdf.links.keys() :
name = toValidID( urdfname )
link = robmodel.connectivity.Link(name)
links[name] = link
children[name] = []
if urdf.links[urdfname].parent == None :
orphans.append(link)
if len(orphans)==0 :
logger.fatal("Could not find any root link (i.e. a link without parent).")
logger.fatal("Check for kinematic loops.")
print("Error, no root link found. Aborting", file=sys.stderr)
sys.exit(-1)
if len(orphans) > 1 :
logger.warning("Found {0} links without parent, only one expected".format(len(orphans)))
logger.warning("Any robot model must have exactly one root element.")
logger.warning("This might lead to unexpected results.")
robotBase = orphans[0]
for jname in urdf.joints.keys() :
urdfjoint = urdf.joints[jname]
name = toValidID( jname )
jkind = urdfjoint.type
if jkind in JointKind.__members__.keys() :
jkind = JointKind[jkind] # otherwise, it stays a string (like 'fixed')
joint = robmodel.connectivity.Joint(name, jkind)
parent = links[ toValidID(urdfjoint.parent) ]
child = links[ toValidID(urdfjoint.child) ]
children[parent.name].append( child )
joints[name] = joint
pairs.append( robmodel.connectivity.KPair(joint, parent, child) )
# CONNECTIVITY MODEL
connectivityModel = robmodel.connectivity.Robot(
urdf.robotName, links, joints, pairs)
# REGULAR NUMBERING
# There is no numbering scheme in the URDF format, so we arbitrarily
# associate code to each link via a Depth-First-Traversal
code = 0
numbering = {}
def setCode(currentLink):
nonlocal code, numbering
if currentLink == None : return
numbering[currentLink.name] = code
for child in children[currentLink.name] :
code = code + 1
setCode( child )
setCode( robotBase )
ordering = { 'robot': robotName, 'nums' : numbering }
orderedModel = robmodel.ordering.Robot(connectivityModel, ordering)
# FRAMES
# The URDF does not have explicit frames, so there are no more frames than
# joints and links; thus the second argument is always the empty list
framesModel = robmodel.frames.RobotDefaultFrames(orderedModel, [])
# GEOMETRY MEASUREMENTS
poses = []
axes = {}
for joint in urdf.joints.values() :
# Get the current joint and link (predecessor)
myjoint = joints[ toValidID( joint.name ) ]
mylink = orderedModel.predecessor(myjoint)
logger.debug("Processing joint * {0} * and predecessor link * {1} *".format(myjoint.name, mylink.name) )
# The relative pose of the URDF joint frame relative to the URDF link frame
xyz, rpy, motion_link_to_joint = linkFrameToJointFrameInURDF(joint)
jaxis = np.round( np.array(joint.frame['axis']), 5)
logger.debug("Joint axis in URDF coordinates : {0}".format(jaxis) )
logger.debug("URDF joint xyz and rpy attributes : {0} {1}".format(xyz, rpy) )
frame_joint = framesModel.framesByName[ myjoint.name ]
frame_link = framesModel.framesByName[ mylink.name ]
pose = primitives.Pose(target=frame_joint, reference=frame_link)
poses.append( PoseSpec(pose, motion_link_to_joint) )
axes[myjoint.name] = joint.frame['axis']
posesModel = motions.PosesSpec(robotName, poses)
geometryModel = robmodel.geometry.Geometry(orderedModel, framesModel, posesModel, axes)
return connectivityModel, orderedModel, framesModel, geometryModel