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
