def build_link(self, node): (id_bda, ta), (id_bdb, tb) = self.link_building_status[node] bda = self.factory.multiboxes[id_bda] bdb = self.factory.multiboxes[id_bdb] pos = bda.body.getPosition() quat = LQuaternionf(bda.body.getQuaternion()) mat = TransformState.makePosQuatScale(pos, quat, Vec3(1, 1, 1)).getMat() mat = ta.getMat() * mat print "ta", ta print "absol", TransformState.makeMat(mat) mat = LMatrix4f.translateMat(Vec3(0.5, 0, 0)) * mat anchor = TransformState.makeMat(mat).getPos() print "absol", TransformState.makeMat(mat) axis = self.quat_dict[1][1] if node.orientation == 1: t = LMatrix4f.rotateMat(*self.quat_dict[4]) * mat else: t = LMatrix4f.rotateMat(*self.quat_dict[2]) * mat row = t.getRow(0) print "rotation", t.getRow(0), type(t.getRow(0)) #axis = t.getQuat().getAxis() axis = Vec3(row.getX(), row.getY(), row.getZ()) print "axis",axis print "anchor", anchor mat = LMatrix4f.translateMat(Vec3(0.5, 0, 0)) * mat mat = tb.getInverse().getMat() * mat t = TransformState.makeMat(mat) posb = t.getPos() quatb = t.getQuat() bdb.body.setPosition(posb) bdb.body.setQuaternion(quatb) cs = OdeHingeJoint(self.physics.world, self.physics.servogroup) cs.attach(bda.body, bdb.body) cs.setAxis(axis) cs.setAnchor(anchor) #add the motor cs.setParamFMax(self.satu_cmd) cs.setParamFudgeFactor(0.5) cs.setParamCFM(11.1111) cs.setParamStopCFM(11.1111) cs.setParamStopERP(0.444444) cs.setParamLoStop(- math.pi * 0.5) cs.setParamHiStop(math.pi * 0.5) pid = PID() self.dof_motors[node] = (cs, pid) print "add constraint"
def change_transform(self, transform, face, type='shape'): """ change to transform to go on a face """ #print " change face to {}".format(face) mat = transform.getMat() mat = LMatrix4f.rotateMat(*self.quat_dict[face]) * mat mat = LMatrix4f.translateMat(Vec3(1.0, 0, 0)) * mat transform = transform.makeMat(mat) return transform
def change_back_transform(self, transform, face, type='bloc'): #print "back transform" mat = transform.getMat() mult = LMatrix4f.rotateMat(*self.quat_dict[face]) mult.invertInPlace() mat = LMatrix4f.translateMat(Vec3(-1.0, 0, 0)) * mat mat = mult * mat transform = transform.makeMat(mat) return transform