def test_6d(self):
a = InnerModelVector.vec6d(1., 2., 3., 4., 5., 6.)
assert (a[0] == 1)
assert (a[5] == 6)
assert (a.shape[0] == 6)
assert (a.min() == [1, 0])
assert (a.max() == [6, 5])
b = InnerModelVector.vec6d(2, 0, 0, 0, 0, 0)
assert (a.dot_product(b) == 2)
assert (b.norm2() == 2)
def transform(self,
destId: str,
origId: str,
origVec: 'InnerModelVector' = None) -> 'InnerModelVector':
'''Get the transformation between two nodes with given ids'''
if origVec is None:
origVec = InnerModelVector.vec3d(0, 0, 0)
if origVec.size() == 3:
m = self.getTransformationMatrix(destId, origId)
v = origVec.toHomogeneousCoordinates()
vt_ = m.dot(v)
vt = InnerModelVector((4, ))
np.copyto(vt, vt_)
return vt.fromHomogeneousCoordinates()
else:
M = self.getTransformationMatrix(destId, origId)
a = M.dot(origVec.subVector(
0, 2).toHomogeneousCoordinates()).fromHomogeneousCoordinates()
R = Rot3D(origVec[3], origVec[4], origVec[5])
b = (M.getSubmatrix(0, 2, 0, 2).dot(R)).extractAnglesR_min()
ret = InnerModelVector.vec6d(a[0], a[1], a[2], b[3], b[4], b[5])
return ret
def recursiveConstructor(self, node, tr, parentId):
'''Recursively construct all the parts in a body'''
if isinstance(node, InnerModelMesh):
self.makeMesh(node, tr, parentId)
elif isinstance(node, InnerModelPlane):
self.makePlane(node, tr, parentId)
elif isinstance(node, (InnerModelRGBD, InnerModelCamera)):
self.makeCamera(node, tr, parentId)
elif isinstance(node, InnerModelLaser):
self.makeLaser(node, tr, parentId)
elif isinstance(node, InnerModelIMU):
self.makeIMU(node, tr, parentId)
elif isinstance(node, (InnerModelJoint, InnerModelPrismaticJoint)):
self.makeJoint(node, tr, parentId)
elif isinstance(node, InnerModelTouchSensor):
self.hasTouchSensor = True
parentId = node.parent.id
self.touchSensorLink.append(parentId)
else:
self.baseMass += node.mass
tr_ = InnerModelVector.vec6d(node.tx / 100.0 + tr.x(),
node.ty / 100.0 + tr.y(),
node.tz / 100.0 + tr.z(),
node.rx + tr.rx(), node.ry + tr.ry(),
node.rz + tr.rz())
for child in node.children:
self.recursiveConstructor(child, tr_, parentId)
def transform6D(self,
destId: str,
orgId: str,
origVec: 'InnerModelVector' = None) -> 'InnerModelVector':
'''Get the transformation between two nodes with given ids'''
if origVec is not None:
assert (origVec.size() == 6)
return self.transform(destId, orgId, origVec)
else:
v = InnerModelVector.vec6d(0, 0, 0, 0, 0, 0)
return self.transform(destId, orgId, v)
def createBody(self, node):
'''Create a separate body from the node'''
assert (self.getClassType(node) == 'Transform')
self.imNode = node
self.id = node.id
# self.baseMass = node.mass
self.baseMass = 10
self.basePosition = [node.tx / 100.0, node.ty / 100.0, node.tz / 100.0]
self.baseOrientation = p.getQuaternionFromEuler(
[node.rx, node.ry, node.rz])
tr = InnerModelVector.vec6d(0, 0, 0, 0, 0, 0)
parentId = 0
for child in node.children:
self.recursiveConstructor(child, tr, parentId)
