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 update_pose(obj_bullet_rbd, objnp): """ update obj_bullet_rbd using the pos, nd quat of objnp :param obj_bullet_rbd: :param objnp: :return: author: weiwei date: 20211215 """ obj_bullet_rbd.setTransform( TransformState.makePosQuatScale(objnp.getPos(), objnp.getQuat(), da.npv3_to_pdv3(np.array([1, 1, 1]))))
def make_fixed(np0, np1, type_=BulletGenericConstraint, cfm=0.01, erp=.99): """ Make a joint and return it.""" t0 = np0.getTop() t1 = np1.getTop() p0 = np0.getPos(t0) p1 = np1.getPos(t1) q0 = np0.getQuat(t0) q1 = np1.getQuat(t1) pivot = Point3((p0 + p1) / 2.) disp = Point3((p1 - p0) / 2.) pivot_np = t0.attachNewNode("pivot-node") pivot_np.setPos(pivot) s = Vec3(1, 1, 1) q = Quat.identQuat() # pivot_np.getQuat(t0) # #BP() # q0 = pivot_np.getQuat(np0) # q1 = pivot_np.getQuat(np1) q0i = Quat(q0) q1i = Quat(q1) #BP() q0i.invertInPlace() q1i.invertInPlace() q0i *= q q1i *= q #BP() ts0 = TransformState.makePosQuatScale(disp, q0i, s) ts1 = TransformState.makePosQuatScale(-disp, q1i, s) pivot_np.removeNode() # Create the joint. joint = type_(np0.node(), np1.node(), ts0, ts1, False) for ax in xrange(4): joint.setAngularLimit(ax, 0, 0) joint.setLinearLimit(ax, 0, 0) joint.setParam(type_.CPErp, erp) joint.setParam(type_.CPCfm, cfm) joint.setDebugDrawSize(2) return joint