def addConstraint(self, position, translation, eulerRotation):
constraint = self.node.createChild("Constraint")
addOrientedBoxRoi(constraint,
position=position,
translation=vec3.vadd(translation, [0.0, 25.0, 0.0]),
eulerRotation=eulerRotation,
scale=[45, 15, 30])
constraint.createObject("TransformEngine",
input_position="@BoxROI.pointsInROI",
translation=translation,
rotation=eulerRotation,
inverse=True)
constraint.createObject("MechanicalObject",
name="dofs",
template="Vec3",
position="@TransformEngine.output_position",
showObject=True,
showObjectScale=10.0)
constraint.createObject('RigidMapping',
name="mapping",
input=self.node.ServoMotor.ServoWheel.dofs,
output="@./")
return constraint
def addBox(self, position, translation, eulerRotation):
constraint = self.node.createChild("Box")
o = addOrientedBoxRoi(constraint,
position=position,
translation=vec3.vadd(translation,
[0.0, 25.0, 0.0]),
eulerRotation=eulerRotation,
scale=[45, 15, 30])
o.init()
def __attachToActuatedArms(self, radius=60, numMotors=3, angleShift=180.0):
deformableObject = self.node.ElasticBody.ElasticMaterialObject
dist = radius
numstep = numMotors
groupIndices = []
frames = []
for i in range(0, numstep):
translation, eulerRotation = self.__getTransform(
i, numstep, angleShift, radius, dist)
box = addOrientedBoxRoi(
self.node,
position=deformableObject.dofs.getData("rest_position"),
name="BoxROI" + str(i),
translation=vec3.vadd(translation, [0.0, 25.0, 0.0]),
eulerRotation=eulerRotation,
scale=[45, 15, 30])
box.drawBoxes = False
box.init()
deformableObject.init()
groupIndices.append([ind[0] for ind in box.indices])
frames.append(
vec3.vadd(translation, [0.0, 25.0, 0.0]) + list(
Quat.createFromEuler(
[0, float(i) * 360 /
float(numstep), 0], inDegree=True)))
# Rigidify the deformable part at extremity to attach arms
rigidifiedstruct = Rigidify(self.node,
deformableObject,
groupIndices=groupIndices,
frames=frames,
name="RigidifiedStructure")
rigidifiedstruct.DeformableParts.createObject(
"UncoupledConstraintCorrection")
rigidifiedstruct.RigidParts.createObject(
"UncoupledConstraintCorrection")
# Use this to activate some rendering on the rigidified object ######################################
# setData(rigidifiedstruct.RigidParts.dofs, showObject=True, showObjectScale=10, drawMode=2)
# setData(rigidifiedstruct.RigidParts.RigidifiedParticules.dofs, showObject=True, showObjectScale=1,
# drawMode=1, showColor=[1., 1., 0., 1.])
# setData(rigidifiedstruct.DeformableParts.dofs, showObject=True, showObjectScale=1, drawMode=2)
#####################################################################################################
# Attach arms
for i in range(0, numstep):
rigidifiedstruct.RigidParts.createObject(
'RestShapeSpringsForceField',
name="rssff" + str(i),
points=i,
external_rest_shape=self.actuatedarms[i].servoarm.dofs,
stiffness='1e12',
angularStiffness='1e12')
def addConstraint(self, position, translation, eulerRotation):
constraint = self.node.createChild("Constraint")
o = addOrientedBoxRoi(constraint, position=position,
translation=vec3.vadd(translation, [0.0, 25.0, 0.0]),
eulerRotation=eulerRotation, scale=[45, 15, 30])
o.drawSize = 1
o.drawBoxes = False
constraint.createObject("TransformEngine", input_position="@BoxROI.pointsInROI",
translation=translation, rotation=eulerRotation, inverse=True)
constraint.createObject("MechanicalObject", name="dofs",
template="Vec3d", position="@TransformEngine.output_position",
showObject=True, showObjectScale=5.0)
constraint.createObject('RigidMapping', name="mapping", input=self.node.ServoMotor.ServoWheel.dofs, output="@./")
return constraint