def onKeyPressed(self, c):
######### Rate angular #########
if c == "+": # +
posA = self.rateAngularDeformMO.findData('rest_position').value
posA[5][1] += self.angularRate
self.rateAngularDeformMO.findData('rest_position').value = posA
if c == "-": # -
posA = self.rateAngularDeformMO.findData('rest_position').value
posA[5][1] -= self.angularRate
self.rateAngularDeformMO.findData('rest_position').value = posA
print("============> The rate angular :", posA)
######### Reste rigid position #########
if ord(c) == 19: # up
posA = self.rigidBaseMO.findData('rest_position').value
qOld = Quat()
for i in range(0, 4):
qOld[i] = posA[0][i + 3]
qNew = Quat.createFromEuler([0., self.angularRate, 0.], 'ryxz')
qNew.normalize()
qNew.rotateFromQuat(qOld)
for i in range(0, 4):
posA[0][i + 3] = qNew[i]
self.rigidBaseMO.findData('rest_position').value = posA
if ord(c) == 21: # down
posA = self.rigidBaseMO.findData('rest_position').value
qOld = Quat()
for i in range(0, 4):
qOld[i] = posA[0][i + 3]
qNew = Quat.createFromEuler([0., -self.angularRate, 0.], 'ryxz')
qNew.normalize()
qNew.rotateFromQuat(qOld)
for i in range(0, 4):
posA[0][i + 3] = qNew[i]
self.rigidBaseMO.findData('rest_position').value = posA
if ord(c) == 18: # left
pos = self.rigidBaseMO.findData('rest_position').value
pos[0][0] -= self.rate
self.rigidBaseMO.findData('rest_position').value = pos
# posA = self.rateAngularDeformMO.findData('position').value
# for i in range(len(posA)):
# posA[i][2] -= self.angularRate
# self.rateAngularDeformMO.findData('position').value = posA
if ord(c) == 20: # right
pos = self.rigidBaseMO.findData('rest_position').value
pos[0][0] += self.rate
self.rigidBaseMO.findData('rest_position').value = pos
def getTranslated(points, vec, angle):
r = []
q = Quat.createFromEuler([(angle[0] * pi) / 180., (angle[1] * pi) / 180.,
(angle[2] * pi) / 180.], 'ryxz')
v = Vec3(vec[0], vec[1], vec[2])
sol = v.rotateFromQuat(q)
for v in points:
r.append([v[0] + sol[0], v[1] + sol[1], v[2] + sol[2]])
return r
## ## ## ## ## ## ## ##
## Cosserat Needs ##
## ## ## ## ## ## ## ##
length1 = 10.
length2 = 2.
lengthTrunk = 195.
nbBeams = 20
nbFrames = 20 # Dans l'ideal >= nbBeams
nbCables = 4
maxSlidingForce = 0
minSlidingForce = -10
pullPoint = [[0., length1, 0.], [-length1, 0., 0.], [0., -length1, 0.], [length1, 0.,0.]]
direction = Vec3( lengthTrunk, length2-length1, 0.0)
direction.normalize()
Ori0 = Quat.createFromEuler([-pi/2., 0., -0.04], 'ryxz')
Ori1 = Quat.createFromEuler([-pi/2.04, 0.0, 0.0], 'ryxz')
Ori2 = Quat.createFromEuler([-pi/2., 0., 0.04], 'ryxz')
Ori3 = Quat.createFromEuler([-pi/1.96, 0., 0.0], 'ryxz')
baseOrientation = [Ori0,Ori1,Ori2,Ori3]
VecCoord = [] # vector of Vec3
VecFrame = [] # vector of Rigid3d
VecCurvAbsOutput = [] # vector of double
VecCurvAbsInput = [] # vector of double
VecLenght = [] # vector of vetor of double; Cosserat beam length
VecPosFem = []
position = [[0., 0., 0.]]*nbBeams
for k in range(0, nbBeams, 2):
position[k] = Vec3(direction[0]*17.5*(k/2)+21, 0.0, 0.0)
def applyAngleToServoWheel(self, angle):
rigidparent = RigidDof(self.parentframe)
rigidtarget = RigidDof(self.target)
rigidtarget.copyFrom(rigidparent)
self.dmap.initialPoints = [0.0, 0.0, 0.0]+list(Quat.createFromEuler([angle, 0,0]))
def __addCables(self):
length1 = 10.
length2 = 2.
lengthTrunk = 195.
pullPoint = [[0., length1, 0.], [-length1, 0., 0.], [0., -length1, 0.],
[length1, 0., 0.]]
direction = Vec3(lengthTrunk, length2 - length1, 0.0)
direction.normalize()
Ori0 = Quat.createFromEuler([-pi / 2., 0., -0.04], 'ryxz')
Ori1 = Quat.createFromEuler([-pi / 2.04, 0.0, 0.0], 'ryxz')
Ori2 = Quat.createFromEuler([-pi / 2., 0., 0.04], 'ryxz')
Ori3 = Quat.createFromEuler([-pi / 1.96, 0., 0.0], 'ryxz')
baseOrientation = [Ori0, Ori1, Ori2, Ori3]
nbCables = 4
for i in range(0, nbCables):
position = [[0., 0., 0.]] * 20
for k in range(0, 20, 2):
position[k] = Vec3(direction[0] * 17.5 * (k / 2) + 21, 0.0,
0.0)
position[k + 1] = Vec3(direction[0] * 17.5 * (k / 2) + 27, 0.0,
0.0)
position = [[0., 0, 0.]] + [pos.toList() for pos in position]
####################################################
### Create the Cosserat cable node
####################################################
distance = self.compute_distance(position)
curv_abs_output = self.createCurvAbsOutput(distance)
curv_abs_input = self.createCurvAbsOutput(distance)
frames = self.createFramesList(position)
# ###############
# RigidBases
###############
basePose = frames[0]
for l in range(0, 3):
basePose[l] = pullPoint[i][l]
for l in range(0, 4):
basePose[l + 3] = baseOrientation[i][l]
rigidBaseNode = self.node.createChild('rigidBase' + str(i))
RigidBaseMO = rigidBaseNode.createObject('MechanicalObject',
template='Rigid3d',
name="RigidBaseMO",
position=basePose,
showObject='1',
showObjectScale='0.6',
showIndices='1')
rigidBaseNode.createObject('RestShapeSpringsForceField',
name='spring',
stiffness="500",
angularStiffness="500",
external_points="0",
mstate="@RigidBaseMO",
points="0",
template="Rigid3d")
#############################################
# Rate of angular Deformation (2 sections)
#############################################
ratPosition = []
for l in range(0, len(frames) - 1):
ratPosition += [0, 0, 0]
longeur = distance # beams size
rateAngularDeformNode = self.node.createChild('rateAngularDeform' +
str(i))
rateAngularDeformMO = rateAngularDeformNode.createObject(
'MechanicalObject',
template='Vec3d',
name='rateAngularDeformMO',
position=ratPosition)
BeamHookeLawForce = rateAngularDeformNode.createObject(
'BeamHookeLawForceField',
crossSectionShape='circular',
length=longeur,
radius='0.5',
youngModulus='5e6',
showIndices='1')
##############
# Frames #
##############
mappedFrameNode = rigidBaseNode.createChild('MappedFrames')
rateAngularDeformNode.addChild(mappedFrameNode)
framesMO = mappedFrameNode.createObject('MechanicalObject',
template='Rigid3d',
name="FramesMO",
position=frames,
showObject='1',
showObjectScale='1',
showIndices=1)
inputMO = rateAngularDeformMO.getLinkPath()
inputMO_rigid = RigidBaseMO.getLinkPath()
outputMO = framesMO.getLinkPath()
mappedFrameNode.createObject('DiscretCosseratMapping',
curv_abs_input=curv_abs_input,
curv_abs_output=curv_abs_output,
input1=inputMO,
input2=inputMO_rigid,
output=outputMO,
debug='0',
printLog=0)
def Rigidify(targetObject,
sourceObject,
groupIndices,
frames=None,
name=None,
frameOrientation=None):
""" Transform a deformable object into a mixed one containing both rigid and deformable parts.
:param targetObject: parent node where to attach the final object.
:param sourceObject: node containing the deformable object. The object should be following
the ElasticMaterialObject template.
:param list groupIndices: array of array indices to rigidify. The length of the array should be equal to the number
of rigid component.
:param list frames: array of frames. The length of the array should be equal to the number
of rigid component. The orientation are given in eulerAngles (in degree) by passing
three values or using a quaternion by passing four values.
[[rx,ry,rz], [qx,qy,qz,w]]
User can also specify the position of the frame by passing six values (position and orientation in degree)
or seven values (position and quaternion).
[[x,y,z,rx,ry,rz], [x,y,z,qx,qy,qz,w]]
If the position is not specified, the position of the rigids will be the barycenter of the region to rigidify.
:param str name: specify the name of the Rigidified object, is none provided use the name of the SOurceObject.
"""
# Deprecation Warning
if frameOrientation is not None:
Sofa.msg_warning(
"The parameter frameOrientations of the function Rigidify is now deprecated. Please use frames instead."
)
frames = frameOrientation
if frames is None:
frames = [[0., 0., 0.]] * len(groupIndices)
assert len(groupIndices) == len(frames), "size mismatch."
if name is None:
name = sourceObject.name
sourceObject.init()
ero = targetObject.createChild(name)
allPositions = sourceObject.container.position
allIndices = map(lambda x: x[0], sourceObject.container.points)
rigids = []
indicesMap = []
def mfilter(si, ai, pts):
tmp = []
for i in ai:
if i in si:
tmp.append(pts[i])
return tmp
# get all the points from the source.
sourcePoints = map(Vec3, sourceObject.dofs.position)
selectedIndices = []
for i in range(len(groupIndices)):
selectedPoints = mfilter(groupIndices[i], allIndices, sourcePoints)
if len(frames[i]) == 3:
orientation = Quat.createFromEuler(frames[i], inDegree=True)
poscenter = getBarycenter(selectedPoints)
elif len(frames[i]) == 4:
orientation = frames[i]
poscenter = getBarycenter(selectedPoints)
elif len(frames[i]) == 6:
orientation = Quat.createFromEuler(
[frames[i][3], frames[i][4], frames[i][5]], inDegree=True)
poscenter = [frames[i][0], frames[i][1], frames[i][2]]
elif len(frames[i]) == 7:
orientation = [
frames[i][3], frames[i][4], frames[i][5], frames[i][6]
]
poscenter = [frames[i][0], frames[i][1], frames[i][2]]
else:
Sofa.msg_error("Do not understand the size of a frame.")
rigids.append(poscenter + list(orientation))
selectedIndices += map(lambda x: x, groupIndices[i])
indicesMap += [i] * len(groupIndices[i])
otherIndices = filter(lambda x: x not in selectedIndices, allIndices)
Kd = {v: None for k, v in enumerate(allIndices)}
Kd.update({v: [0, k] for k, v in enumerate(otherIndices)})
Kd.update({v: [1, k] for k, v in enumerate(selectedIndices)})
indexPairs = [v for kv in Kd.values() for v in kv]
freeParticules = ero.createChild("DeformableParts")
freeParticules.createObject(
"MechanicalObject",
template="Vec3",
name="dofs",
position=[allPositions[i] for i in otherIndices])
rigidParts = ero.createChild("RigidParts")
rigidParts.createObject("MechanicalObject",
template="Rigid3",
name="dofs",
reserve=len(rigids),
position=rigids)
rigidifiedParticules = rigidParts.createChild("RigidifiedParticules")
rigidifiedParticules.createObject(
"MechanicalObject",
template="Vec3",
name="dofs",
position=[allPositions[i] for i in selectedIndices])
rigidifiedParticules.createObject("RigidMapping",
name="mapping",
globalToLocalCoords='true',
rigidIndexPerPoint=indicesMap)
sourceObject.removeObject(sourceObject.solver)
sourceObject.removeObject(sourceObject.integration)
sourceObject.removeObject(sourceObject.LinearSolverConstraintCorrection)
# The coupling is made with the sourceObject. If the source object is from an ElasticMaterialObject
# We need to get the owning node form the current python object (this is a hack because of the not yet
# Finalized design of stlib.
coupling = sourceObject
if hasattr(sourceObject, "node"):
coupling = sourceObject.node
coupling.createObject("SubsetMultiMapping",
name="mapping",
template="Vec3,Vec3",
input=freeParticules.dofs.getLinkPath() + " " +
rigidifiedParticules.dofs.getLinkPath(),
output=sourceObject.dofs.getLinkPath(),
indexPairs=indexPairs)
rigidifiedParticules.addChild(coupling)
freeParticules.addChild(coupling)
return ero
def Gripper(parentNode=None, **kwargs):
selfNode = parentNode.createChild("Gripper")
translation = kwargs.pop('translation')
rotation = kwargs.pop('rotation')
color = kwargs.pop('color')
#WARNING: Quat createFromEuler will modify the input Euler list!!!
rotation_copy = [r for r in rotation]
rotBase = Quat.createFromEuler(rotation_copy, inDegree=True)
transBase = Vec3(translation)
scale = 0.05
dimBase = Vec3([1. * scale, .2 * scale, 2 * scale])
dimLink = Vec3([1. * scale, 2. * scale, .4 * scale])
offsetLink1 = Vec3([0.0, 2.2 * scale, -1.6 * scale])
offsetLink2 = Vec3([0.0, 2.2 * scale, 1.6 * scale])
translation_base = transBase.rotateFromQuat(rotBase).toList()
translation_link1 = vadd(transBase.toList(),
offsetLink1.rotateFromQuat(rotBase).toList())
translation_link2 = vadd(transBase.toList(),
offsetLink2.rotateFromQuat(rotBase).toList())
base = Cube(selfNode,
name='base',
uniformScale=dimBase.toList(),
translation=translation_base,
rotation=rotation,
color=color)
#WARNING: rotateFromQuat is also an in-place operation, why do they do this?!
attachOffset_translation1 = [0.0, .2 * scale, -1.6 * scale]
attachOffset_translation2 = [0.0, .2 * scale, 1.6 * scale]
baseAttaches = base.createChild('baseAttaches')
baseAttaches.createObject(
'MechanicalObject',
name='attaches',
template='Rigid3d',
# translation2=attachOffset_translation1+attachOffset_translation2, rotation2=rotation+rotation
position=ListToString(attachOffset_translation1) + ' 0 0 0 1 ' +
ListToString(attachOffset_translation2) + ' 0 0 0 1')
baseAttaches.createObject('RigidRigidMapping')
base.getChild('collision').getObject("TTriangleModel").findData(
'group').value = '1 2'
base.getChild('collision').getObject("TLineModel").findData(
'group').value = '1 2'
base.getChild('collision').getObject("TPointModel").findData(
'group').value = '1 2'
baseInput = base.createChild('baseInput')
baseInput.createObject('MechanicalObject',
name='input',
template='Rigid3d',
translation=translation_base,
rotation=rotation)
# baseInput.createObject('JointSpringForceField', object1='@./', object2='@../',
# spring='BEGIN_SPRING 0 0 FREE_AXIS 0 0 0 0 0 0 KS_T 20000 30000 KS_R 150000 200000 KD 1 END_SPRING',
# name='base_input', template='Rigid3d')
baseInput.createObject('AttachConstraint',
indices1='0',
name='attachConstraint',
indices2='0',
velocityFactor='1.0',
responseFactor='1.0',
object1='@./',
object2='@../',
positionFactor='1.0',
constraintFactor='1',
twoWay='false')
link1 = Cube(selfNode,
name='link1',
uniformScale=dimLink.toList(),
translation=translation_link1,
rotation=rotation,
color=color)
attachOffset_translation = [0.0, -2. * scale, 0]
link1Attaches = link1.createChild('link1Attaches')
link1Attaches.createObject(
'MechanicalObject',
name='attaches',
template='Rigid3d',
position=ListToString(attachOffset_translation) + ' 0 0 0 1 ')
link1Attaches.createObject('RigidRigidMapping')
link1.getChild('collision').getObject("TTriangleModel").findData(
'group').value = '1'
link1.getChild('collision').getObject("TLineModel").findData(
'group').value = '1'
link1.getChild('collision').getObject("TPointModel").findData(
'group').value = '1'
link2 = Cube(selfNode,
name='link2',
uniformScale=dimLink.toList(),
translation=translation_link2,
rotation=rotation,
color=color)
attachOffset_translation = [0.0, -2. * scale, 0]
link2Attaches = link2.createChild('link2Attaches')
link2Attaches.createObject(
'MechanicalObject',
name='attaches',
template='Rigid3d',
position=ListToString(attachOffset_translation) + ' 0 0 0 1 ')
link2Attaches.createObject('RigidRigidMapping')
link2.getChild('collision').getObject("TTriangleModel").findData(
'group').value = '2'
link2.getChild('collision').getObject("TLineModel").findData(
'group').value = '2'
link2.getChild('collision').getObject("TPointModel").findData(
'group').value = '2'
link1.createObject(
'JointSpringForceField',
object1="@../base/baseAttaches",
object2="@./link1Attaches",
spring=
'BEGIN_SPRING 0 0 FREE_AXIS 1 1 1 0 0 0 KS_T 20000 30000 KS_R 150000 200000 KD 100 REST_T 50 50 0.0 END_SPRING',
name='joint_link1',
template='Rigid3d')
link2.createObject(
'JointSpringForceField',
object1="@../base/baseAttaches",
object2="@./link2Attaches",
spring=
'BEGIN_SPRING 1 0 FREE_AXIS 1 1 1 0 0 0 KS_T 20000 30000 KS_R 150000 200000 KD 100 REST_T 0.0 50 50 END_SPRING',
name='joint_link2',
template='Rigid3d')
# link1.createObject('AttachConstraint', indices1='0', name='attachConstraint', indices2='0',
# velocityFactor='1.0', responseFactor='1.0', object1='@./link1Attaches', object2='@../base/baseAttaches', positionFactor='1.0', constraintFactor='1', twoWay='true')
# link2.createObject('AttachConstraint', indices1='0', name='attachConstraint', indices2='1',
# velocityFactor='1.0', responseFactor='1.0', object1='@./link2Attaches', object2='@../base/baseAttaches', positionFactor='1.0', constraintFactor='1', twoWay='true')
GripperController(selfNode, base, link1, link2)
return selfNode