def __attachToActuatedArms(self, radius=60, numMotors=3, angleShift=180.0):
deformableObject = self.elasticMaterialObject
self.ElasticBody.init()
dist = radius
numstep = numMotors
groupIndices = []
frames = []
for i in range(0, numstep):
translation, eulerRotation = self.__getTransform(
i, numstep, angleShift, radius, dist)
box = addOrientedBoxRoi(
self,
position=[
list(i) for i in deformableObject.dofs.rest_position.value
],
name="BoxROI" + str(i),
translation=vec3.vadd(translation, [0.0, 25.0, 0.0]),
eulerRotation=eulerRotation,
scale=[45, 15, 30])
box.drawBoxes = True
box.init()
# deformableObject.init()
groupIndices.append([ind for ind in box.indices.value])
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,
deformableObject,
groupIndices=groupIndices,
frames=frames,
name="RigidifiedStructure")
rigidifiedstruct.DeformableParts.addObject(
"UncoupledConstraintCorrection")
rigidifiedstruct.RigidParts.addObject("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.addObject(
'RestShapeSpringsForceField',
name="rssff" + str(i),
points=i,
external_rest_shape=self.actuatedarms[i].servoarm.dofs.
getLinkPath(),
stiffness=1e12,
angularStiffness=1e12)
def addConstraint(self, deformableObject, translation, eulerRotation):
constraint = self.addChild('Constraint')
o = addOrientedBoxRoi(
constraint,
position=deformableObject.dofs.getData('rest_position'),
translation=vec3.vadd(translation, [0.0, 25.0, 0.0]),
eulerRotation=eulerRotation,
scale=[45, 15, 30])
o.drawSize = 1
o.drawBoxes = False
constraint.addObject('TransformEngine',
input_position='@BoxROI.pointsInROI',
translation=translation,
rotation=eulerRotation,
inverse=True)
constraint.addObject('MechanicalObject',
name='dofs',
template='Vec3',
position='@TransformEngine.output_position',
showObject=True,
showObjectScale=5.0)
constraint.addObject('RigidMapping',
name='mapping',
input=self.ServoMotor.ServoWheel.dofs,
output='@./')
return constraint
def addConstraint(self, position, translation, eulerRotation):
constraint = self.node.addChild("Constraint")
addOrientedBoxRoi(constraint,
position=position,
translation=vec3.vadd(translation, [0.0, 25.0, 0.0]),
eulerRotation=eulerRotation,
scale=[45, 15, 30])
constraint.addObject("TransformEngine",
input_position="@BoxROI.pointsInROI",
translation=translation,
rotation=eulerRotation,
inverse=True)
constraint.addObject("MechanicalObject",
name="dofs",
template="Vec3",
position="@TransformEngine.output_position",
showObject=True,
showObjectScale=10.0)
constraint.addObject('RigidMapping',
name="mapping",
input=self.node.ServoMotor.ServoWheel.dofs,
output="@./")
return constraint
def addBox(self, position, translation, eulerRotation):
constraint = self.addChild('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 addBox(self, position, translation, eulerRotation):
constraint = self.node.addChild("Box")
o = addOrientedBoxRoi(constraint,
position=position,
translation=vec3.vadd(translation,
[0.0, 25.0, 0.0]),
eulerRotation=eulerRotation,
scale=[45, 15, 30])
o.drawBoxes = False
o.init()