def addAnchor(self,
nodeIndex=-1,
bodyUniqueId=-1,
linkIndex=-1,
bodyFramePosition=[0, 0, 0],
physicsClientId=0):
p.createSoftBodyAnchor(self.body_id, nodeIndex, bodyUniqueId,
linkIndex, bodyFramePosition, physicsClientId)
def activate_def(self, def_id):
"""Simulates suction by anchoring vertices of the deformable object.
Get distance values in `distances`, get indices for argsort, then
resulting indices in `distances_sort` correspond _exactly_ to vertex
indices arranged from nearest to furthest to the gripper.
Args:
def_id: bullet id for object to anchor.
Returns:
info dict containing activation information.
"""
_, vert_pos_l = p.getMeshData(bodyUniqueId=def_id)
gripper_position = np.float32(p.getLinkState(self.body, 0)[0])
distances = []
for v_position in vert_pos_l:
d = gripper_position - np.float32(v_position)
distances.append(np.linalg.norm(d))
distances_sort = np.argsort(distances)
anchors = []
for i in range(self.def_nb_anchors):
# For each vertex close enough (under threshold), create anchor(s).
v_index = distances_sort[i]
if distances[v_index] > self.def_threshold:
pass
# This should prevent us from gripping if the suction didn't grip
# anything.
if distances[v_index] > self.def_ignore:
print(
f'WARNING, dist={distances[v_index]:0.4f} > thresh '
f'{self.def_ignore:0.4f}. No points are close to the suction'
)
break
anchor_id = p.createSoftBodyAnchor(
softBodyBodyUniqueId=def_id,
nodeIndex=v_index,
bodyUniqueId=self.body,
linkIndex=-1,
)
anchors.append(anchor_id)
info = {
'anchors': anchors,
'closest_vertex': distances_sort[0],
'min_distance': np.min(distances),
}
return info
def activate_def(self, defId):
"""Simulates suction by anchoring vertices of the deformable object.
Get distance values in `distances`, get indices for argsort, then
resulting indices in `distances_sort` correspond _exactly_ to vertex
indices arranged from nearest to furthest to the gripper.
"""
_, vert_pos_l = p.getMeshData(defId,
-1,
flags=p.MESH_DATA_SIMULATION_MESH)
gripper_position = np.float32(p.getLinkState(self.body, 0)[0])
distances = []
for v_position in vert_pos_l:
d = gripper_position - np.float32(v_position)
distances.append(np.linalg.norm(d))
distances_sort = np.argsort(distances)
anchors = []
for i in range(self.def_nb_anchors):
# For each vertex close enough (under threshold), create anchor(s).
vIndex = distances_sort[i]
if distances[vIndex] > self.def_threshold:
#print(f'WARNING, dist={distances[vIndex]:0.4f} > {self.def_threshold} '
# f'This means our gripper touched the surface (z=0)')
pass
# This should prevent us from gripping if the suction didn't grip anything.
if distances[vIndex] > self.def_ignore:
print(
f'WARNING, dist={distances[vIndex]:0.4f} > thresh '
f'{self.def_ignore:0.4f}. No points are close to the suction'
)
break
anchorId = p.createSoftBodyAnchor(
softBodyBodyUniqueId=defId,
nodeIndex=vIndex,
bodyUniqueId=self.body,
linkIndex=-1,
)
anchors.append(anchorId)
info = {
'anchors': anchors,
'closest_vertex': distances_sort[0],
'min_distance': np.min(distances),
}
#print(info)
return info
useMassSpring=1,
springElasticStiffness=121,
springDampingStiffness=1,
springDampingAllDirections=1,
useSelfCollision=1,
frictionCoeff=0,
useFaceContact=1)
p.changeVisualShape(bodyId, -1, rgbaColor=[160 / 255, 69 / 255, 19 / 255, 1])
p.changeVisualShape(armId1, -1, rgbaColor=[0, 0, 0, 0.8])
p.changeVisualShape(armId2, -1, rgbaColor=[0, 0, 0, 0.8])
p.changeVisualShape(armId3, -1, rgbaColor=[0, 0, 0, 0.8])
p.changeVisualShape(armId4, -1, rgbaColor=[0, 0, 0, 0.8])
# attach bodies
p.createSoftBodyAnchor(armId1, 7, bodyId, -1)
p.createSoftBodyAnchor(armId1, 8, bodyId, -1)
p.createSoftBodyAnchor(armId1, 16, bodyId, -1)
p.createSoftBodyAnchor(armId1, 17, bodyId, -1)
p.createSoftBodyAnchor(armId2, 7, bodyId, -1)
p.createSoftBodyAnchor(armId2, 8, bodyId, -1)
p.createSoftBodyAnchor(armId2, 16, bodyId, -1)
p.createSoftBodyAnchor(armId2, 17, bodyId, -1)
p.createSoftBodyAnchor(armId3, 0, bodyId, -1)
p.createSoftBodyAnchor(armId3, 1, bodyId, -1)
p.createSoftBodyAnchor(armId3, 2, bodyId, -1)
p.createSoftBodyAnchor(armId3, 3, bodyId, -1)
p.createSoftBodyAnchor(armId4, 0, bodyId, -1)
def add_cable_ring(self, env):
"""Make the cable beads coincide with the vertices of the top ring.
Should lead to better physics and will make it easy for an algorithm
to see the bag's top ring. Notable differences between this and the
cables: (1) we don't need to discretize rotations and manually
compute bead positions, because the previously created bag does it
for us, (2) Beads have anchors with vertices, in ADDITION to
constraints with adjacent beads.
Args:
env: A ravens environment.
"""
num_parts = len(self._top_ring_idxs)
radius = 0.005
color = U.COLORS['blue'] + [1]
beads = []
bead_positions_l = []
part_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[radius] * 3)
part_visual = p.createVisualShape(p.GEOM_SPHERE, radius=radius * 1.5)
# Fortunately `verts_l` coincides with `self._top_ring_idxs`.
_, verts_l = p.getMeshData(self.bag_id)
# Iterate through parts and create constraints as needed.
for i in range(num_parts):
bag_vidx = self._top_ring_idxs[i]
bead_position = np.float32(verts_l[bag_vidx])
part_id = p.createMultiBody(
0.01, part_shape, part_visual, basePosition=bead_position)
p.changeVisualShape(part_id, -1, rgbaColor=color)
if i > 0:
parent_frame = bead_position - bead_positions_l[-1]
constraint_id = p.createConstraint(
parentBodyUniqueId=beads[-1],
parentLinkIndex=-1,
childBodyUniqueId=part_id,
childLinkIndex=-1,
jointType=p.JOINT_POINT2POINT,
jointAxis=(0, 0, 0),
parentFramePosition=parent_frame,
childFramePosition=(0, 0, 0))
p.changeConstraint(constraint_id, maxForce=100)
# Make a constraint with i=0. Careful with `parent_frame`!
if i == num_parts - 1:
parent_frame = bead_positions_l[0] - bead_position
constraint_id = p.createConstraint(
parentBodyUniqueId=part_id,
parentLinkIndex=-1,
childBodyUniqueId=beads[0],
childLinkIndex=-1,
jointType=p.JOINT_POINT2POINT,
jointAxis=(0, 0, 0),
parentFramePosition=parent_frame,
childFramePosition=(0, 0, 0))
p.changeConstraint(constraint_id, maxForce=100)
# Create constraint between a bead and certain bag vertices.
_ = p.createSoftBodyAnchor(
softBodyBodyUniqueId=self.bag_id,
nodeIndex=bag_vidx,
bodyUniqueId=part_id,
linkIndex=-1)
# Track beads.
beads.append(part_id)
bead_positions_l.append(bead_position)
# Add objects in a consistent manner.
self.cable_bead_ids.append(part_id)
env.objects.append(part_id)
self.object_points[part_id] = np.float32((0, 0, 0)).reshape(3, 1)
def resetSim():
p.setGravity(0,0,0)
planeId = p.loadURDF("plane.urdf",[0,0,0])
# create bodies
mod1 = p.loadURDF("cube_small.urdf",[body_length/2+arm_width+arm_length+0.25,body_width/2-arm_width/2,2.5],globalScaling=5)
p.changeVisualShape(mod1,-1,rgbaColor=[1,0,0,1])
mod2 = p.loadURDF("cube_small.urdf",[body_length/2+arm_width+arm_length+0.25,-body_width/2+arm_width/2,arm_height/2],globalScaling=5)
p.changeVisualShape(mod2,-1,rgbaColor=[0,1,0,1])
mod3 = p.loadURDF("cube_small.urdf",[-arm_length-body_length/2-arm_width/2-0.75,body_width/2-arm_width/2,arm_height/2],globalScaling=5)
p.changeVisualShape(mod3,-1,rgbaColor=[1,0,1,1])
mod4 = p.loadURDF("cube_small.urdf",[-arm_length-body_length/2-arm_width/2-0.75,-body_width/2+arm_width/2,arm_height/2],globalScaling=5)
p.changeVisualShape(mod4,-1,rgbaColor=[0,0,1,1])
corrFac = 0.05
bodyId = p.loadURDF("body.urdf", [0,0,2.5])
armId1 = p.loadSoftBody("arm_hori.obj", basePosition = [body_length/2+arm_width/2+corrFac,body_width/2-arm_width/2,arm_height/2], scale = 0.1, mass = 1., useNeoHookean = 1, useBendingSprings=1, useMassSpring=1, springElasticStiffness=121, springDampingStiffness=1, springDampingAllDirections = 1, useSelfCollision = 1, frictionCoeff = 0, useFaceContact=1)
armId2 = p.loadSoftBody("arm_hori.obj", basePosition = [body_length/2+arm_width/2+corrFac,-body_width/2+arm_width/2,arm_height/2], scale = 0.1, mass = 1., useNeoHookean = 1, useBendingSprings=1, useMassSpring=1, springElasticStiffness=121, springDampingStiffness=1, springDampingAllDirections = 1, useSelfCollision = 1, frictionCoeff = 0, useFaceContact=1)
armId3 = p.loadSoftBody("arm_hori.obj", basePosition = [-arm_length-body_length/2-arm_width/2-corrFac,body_width/2-arm_width/2,arm_height/2], scale = 0.1, mass = 1., useNeoHookean = 1, useBendingSprings=1, useMassSpring=1, springElasticStiffness=121, springDampingStiffness=1, springDampingAllDirections = 1, useSelfCollision = 1, frictionCoeff = 0, useFaceContact=1)
armId4 = p.loadSoftBody("arm_hori.obj", basePosition = [-arm_length-body_length/2-arm_width/2-corrFac,-body_width/2+arm_width/2,arm_height/2], scale = 0.1, mass = 1., useNeoHookean = 1, useBendingSprings=1, useMassSpring=1, springElasticStiffness=121, springDampingStiffness=1, springDampingAllDirections = 1, useSelfCollision = 1, frictionCoeff = 0, useFaceContact=1)
p.changeVisualShape(bodyId,-1,rgbaColor=[160/255,69/255,19/255,1])
p.changeVisualShape(armId1,-1,rgbaColor=[0,0,0,0.8])
p.changeVisualShape(armId2,-1,rgbaColor=[0,0,0,0.8])
p.changeVisualShape(armId3,-1,rgbaColor=[0,0,0,0.8])
p.changeVisualShape(armId4,-1,rgbaColor=[0,0,0,0.8])
# attach bodies
p.createSoftBodyAnchor(armId1,7,bodyId,-1)
p.createSoftBodyAnchor(armId1,8,bodyId,-1)
p.createSoftBodyAnchor(armId1,16,bodyId,-1)
p.createSoftBodyAnchor(armId1,17,bodyId,-1)
p.createSoftBodyAnchor(armId2,7,bodyId,-1)
p.createSoftBodyAnchor(armId2,8,bodyId,-1)
p.createSoftBodyAnchor(armId2,16,bodyId,-1)
p.createSoftBodyAnchor(armId2,17,bodyId,-1)
p.createSoftBodyAnchor(armId3,0,bodyId,-1)
p.createSoftBodyAnchor(armId3,1,bodyId,-1)
p.createSoftBodyAnchor(armId3,2,bodyId,-1)
p.createSoftBodyAnchor(armId3,3,bodyId,-1)
p.createSoftBodyAnchor(armId4,0,bodyId,-1)
p.createSoftBodyAnchor(armId4,1,bodyId,-1)
p.createSoftBodyAnchor(armId4,2,bodyId,-1)
p.createSoftBodyAnchor(armId4,3,bodyId,-1)
p.createSoftBodyAnchor(armId1,0,mod1,-1)
p.createSoftBodyAnchor(armId1,1,mod1,-1)
p.createSoftBodyAnchor(armId1,2,mod1,-1)
p.createSoftBodyAnchor(armId1,3,mod1,-1)
p.createSoftBodyAnchor(armId2,0,mod2,-1)
p.createSoftBodyAnchor(armId2,1,mod2,-1)
p.createSoftBodyAnchor(armId2,2,mod2,-1)
p.createSoftBodyAnchor(armId2,3,mod2,-1)
p.createSoftBodyAnchor(armId3,7,mod3,-1)
p.createSoftBodyAnchor(armId3,8,mod3,-1)
p.createSoftBodyAnchor(armId3,16,mod3,-1)
p.createSoftBodyAnchor(armId3,17,mod3,-1)
p.createSoftBodyAnchor(armId4,7,mod4,-1)
p.createSoftBodyAnchor(armId4,8,mod4,-1)
p.createSoftBodyAnchor(armId4,16,mod4,-1)
p.createSoftBodyAnchor(armId4,17,mod4,-1)
modArr = [mod1,mod2,mod3,mod4]
armArr = [armId1,armId2,armId3,armId4]
partList = [planeId,bodyId,mod1,mod2,mod3,mod4,armId1,armId2,armId3,armId4]
# run simulation
useRealTimeSimulation = 0
if (useRealTimeSimulation):
p.setRealTimeSimulation(0)
return modArr,armArr,partList
def startSim():
posArr1 = []
posArr2 = []
posArr3 = []
posArr4 = []
posArr5 = []
velArr1 = []
velArr2 = []
velArr3 = []
velArr4 = []
# create bodies
mod1 = p.loadURDF("cube_small.urdf",[body_length/2+arm_width+arm_length+0.07,body_width/2-arm_width/2,arm_height/2],globalScaling=1./sF)
p.changeVisualShape(mod1,-1,rgbaColor=[1,0,0,1])
mod2 = p.loadURDF("cube_small.urdf",[body_length/2+arm_width+arm_length+0.07,-body_width/2+arm_width/2,arm_height/2],globalScaling=1./sF)
p.changeVisualShape(mod2,-1,rgbaColor=[0,1,0,1])
mod3 = p.loadURDF("cube_small.urdf",[-arm_length-body_length/2-arm_width/2-0.10,body_width/2-arm_width/2,arm_height/2],globalScaling=1./sF)
p.changeVisualShape(mod3,-1,rgbaColor=[1,0,1,1])
mod4 = p.loadURDF("cube_small.urdf",[-arm_length-body_length/2-arm_width/2-0.10,-body_width/2+arm_width/2,arm_height/2],globalScaling=1./sF)
p.changeVisualShape(mod4,-1,rgbaColor=[0,0,1,1])
corrFac = 0.02
bodyId = p.loadURDF("body.urdf", [0,0,body_height],globalScaling=0.1/sF)
armId1 = p.loadSoftBody("arm_hori.obj", basePosition = [body_length/2+arm_width/2+corrFac,body_width/2-arm_width/2,arm_height/2], scale = 0.01/sF, mass = 1., useNeoHookean = 1, useBendingSprings=1, useMassSpring=1, springElasticStiffness=121, springDampingStiffness=1, springDampingAllDirections = 1, useSelfCollision = 1, frictionCoeff = 0, useFaceContact=1)
armId2 = p.loadSoftBody("arm_hori.obj", basePosition = [body_length/2+arm_width/2+corrFac,-body_width/2+arm_width/2,arm_height/2], scale = 0.01/sF, mass = 1., useNeoHookean = 1, useBendingSprings=1, useMassSpring=1, springElasticStiffness=121, springDampingStiffness=1, springDampingAllDirections = 0, useSelfCollision = 1, frictionCoeff = 0.1, useFaceContact=1)
armId3 = p.loadSoftBody("arm_hori.obj", basePosition = [-arm_length-body_length/2-arm_width/2-corrFac,body_width/2-arm_width/2,arm_height/2], scale = 0.01/sF, mass = 1., useNeoHookean = 1, useBendingSprings=1, useMassSpring=1, springElasticStiffness=121, springDampingStiffness=1, springDampingAllDirections = 1, useSelfCollision = 1, frictionCoeff = 0, useFaceContact=1)
armId4 = p.loadSoftBody("arm_hori.obj", basePosition = [-arm_length-body_length/2-arm_width/2-corrFac,-body_width/2+arm_width/2,arm_height/2], scale = 0.01/sF, mass = 1., useNeoHookean = 1, useBendingSprings=1, useMassSpring=1, springElasticStiffness=121, springDampingStiffness=1, springDampingAllDirections = 1, useSelfCollision = 1, frictionCoeff = 0, useFaceContact=1)
p.changeVisualShape(bodyId,-1,rgbaColor=[160/255,69/255,19/255,1])
#p.changeVisualShape(armId1,-1,rgbaColor=[0,0,0,0.8])
#p.changeVisualShape(armId2,-1,rgbaColor=[0,0,0,0.8])
#p.changeVisualShape(armId3,-1,rgbaColor=[0,0,0,0.8])
#p.changeVisualShape(armId4,-1,rgbaColor=[0,0,0,0.8])
# attach bodies
p.createSoftBodyAnchor(armId1,7,bodyId,-1)
p.createSoftBodyAnchor(armId1,8,bodyId,-1)
p.createSoftBodyAnchor(armId1,16,bodyId,-1)
p.createSoftBodyAnchor(armId1,17,bodyId,-1)
p.createSoftBodyAnchor(armId2,7,bodyId,-1)
p.createSoftBodyAnchor(armId2,8,bodyId,-1)
p.createSoftBodyAnchor(armId2,16,bodyId,-1)
p.createSoftBodyAnchor(armId2,17,bodyId,-1)
p.createSoftBodyAnchor(armId3,0,bodyId,-1)
p.createSoftBodyAnchor(armId3,1,bodyId,-1)
p.createSoftBodyAnchor(armId3,2,bodyId,-1)
p.createSoftBodyAnchor(armId3,3,bodyId,-1)
p.createSoftBodyAnchor(armId4,0,bodyId,-1)
p.createSoftBodyAnchor(armId4,1,bodyId,-1)
p.createSoftBodyAnchor(armId4,2,bodyId,-1)
p.createSoftBodyAnchor(armId4,3,bodyId,-1)
p.createSoftBodyAnchor(armId1,0,mod1,-1)
p.createSoftBodyAnchor(armId1,1,mod1,-1)
p.createSoftBodyAnchor(armId1,2,mod1,-1)
p.createSoftBodyAnchor(armId1,3,mod1,-1)
p.createSoftBodyAnchor(armId2,0,mod2,-1)
p.createSoftBodyAnchor(armId2,1,mod2,-1)
p.createSoftBodyAnchor(armId2,2,mod2,-1)
p.createSoftBodyAnchor(armId2,3,mod2,-1)
p.createSoftBodyAnchor(armId3,7,mod3,-1)
p.createSoftBodyAnchor(armId3,8,mod3,-1)
p.createSoftBodyAnchor(armId3,16,mod3,-1)
p.createSoftBodyAnchor(armId3,17,mod3,-1)
p.createSoftBodyAnchor(armId4,7,mod4,-1)
p.createSoftBodyAnchor(armId4,8,mod4,-1)
p.createSoftBodyAnchor(armId4,16,mod4,-1)
p.createSoftBodyAnchor(armId4,17,mod4,-1)
modList = [mod1,mod2,mod3,mod4]
return modList,bodyId
def load_world(self,
spring_elastic_stiffness,
spring_damping_stiffness,
spring_bending_stiffness,
rand_pos=False):
"""
init plane and table of the environment
:param spring_elastic_stiffness
:param spring_damping_stiffness
:param spring_bending_stiffness
:return:
"""
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# reset the environment to enable simulate deformable object
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW)
p.setGravity(0, 0, -10)
plane_id = p.loadURDF('plane.urdf')
# table_id = p.loadURDF(TABLE_PATH, basePosition=self.table_position,
# baseOrientation=p.getQuaternionFromEuler([0, 0, np.pi / 2.0]),
# flags=p.URDF_USE_SELF_COLLISION_INCLUDE_PARENT)
# init cloth
cloth_position = self.cloth_position
if rand_pos:
pos_ = np.random.uniform(low=-0.25, high=0.25, size=(2, ))
cloth_id = p.loadSoftBody(
CLOTH_PATH,
basePosition=cloth_position + [pos_[0], pos_[1], 0],
scale=0.2,
mass=1.,
useNeoHookean=0,
useBendingSprings=1,
useMassSpring=1,
useSelfCollision=1,
springElasticStiffness=spring_elastic_stiffness,
springDampingStiffness=spring_damping_stiffness,
springBendingStiffness=spring_bending_stiffness,
springDampingAllDirections=1,
frictionCoeff=.5,
useFaceContact=1)
else:
cloth_id = p.loadSoftBody(
CLOTH_PATH,
basePosition=cloth_position,
scale=0.2,
mass=1.,
useNeoHookean=0,
useBendingSprings=1,
useMassSpring=1,
useSelfCollision=1,
springElasticStiffness=spring_elastic_stiffness,
springDampingStiffness=spring_damping_stiffness,
springBendingStiffness=spring_bending_stiffness,
springDampingAllDirections=1,
frictionCoeff=.5,
useFaceContact=1)
anchor_1 = p.createSoftBodyAnchor(cloth_id, 0, -1, -1)
anchor_2 = p.createSoftBodyAnchor(cloth_id, 1, -1, -1)
return plane_id, cloth_id, (anchor_1, anchor_2)
p.setGravity(0, 0, gravZ)
planeOrn = [0, 0, 0, 1] #p.getQuaternionFromEuler([0.3,0,0])
planeId = p.loadURDF("plane.urdf", [0, 0, -2], planeOrn)
boxId = p.loadURDF("cube.urdf", [0, 1, 2], useMaximalCoordinates=True)
clothId = p.loadSoftBody("cloth_z_up.obj",
basePosition=[0, 0, 2],
scale=0.5,
mass=1.,
useNeoHookean=0,
useBendingSprings=1,
useMassSpring=1,
springElasticStiffness=40,
springDampingStiffness=.1,
useSelfCollision=0,
frictionCoeff=.5,
useFaceContact=1)
p.createSoftBodyAnchor(clothId, 0, -1, -1)
p.createSoftBodyAnchor(clothId, 1, -1, -1)
p.createSoftBodyAnchor(clothId, 3, boxId, -1, [0.5, -0.5, 0])
p.createSoftBodyAnchor(clothId, 2, boxId, -1, [-0.5, -0.5, 0])
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
p.setRealTimeSimulation(1)
while p.isConnected():
p.setGravity(0, 0, gravZ)
sleep(1. / 240.)
-body_width / 2 + arm_width / 2, arm_height / 2
],
scale=0.1,
mass=1.,
useNeoHookean=0,
useBendingSprings=1,
useMassSpring=1,
springElasticStiffness=40,
springDampingStiffness=.1,
springDampingAllDirections=1,
useSelfCollision=0,
frictionCoeff=0,
useFaceContact=1)
# attach bodies
p.createSoftBodyAnchor(armId1, 7, bodyId, -1)
p.createSoftBodyAnchor(armId1, 8, bodyId, -1)
p.createSoftBodyAnchor(armId1, 16, bodyId, -1)
p.createSoftBodyAnchor(armId1, 17, bodyId, -1)
p.createSoftBodyAnchor(armId2, 7, bodyId, -1)
p.createSoftBodyAnchor(armId2, 8, bodyId, -1)
p.createSoftBodyAnchor(armId2, 16, bodyId, -1)
p.createSoftBodyAnchor(armId2, 17, bodyId, -1)
p.createSoftBodyAnchor(armId3, 0, bodyId, -1)
p.createSoftBodyAnchor(armId3, 1, bodyId, -1)
p.createSoftBodyAnchor(armId3, 2, bodyId, -1)
p.createSoftBodyAnchor(armId3, 3, bodyId, -1)
p.createSoftBodyAnchor(armId4, 0, bodyId, -1)
gravZ = -10
p.setGravity(0, 0, gravZ)
planeOrn = [0, 0, 0, 1] # p.getQuaternionFromEuler([0.3,0,0])
# planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
boxId = p.loadURDF("cube.urdf", [0, 1, 2], useMaximalCoordinates=True)
clothId = p.loadSoftBody("cloth_z_up.obj", basePosition=[0, 0, 2], scale=0.5, mass=1., useNeoHookean=0,
useBendingSprings=1, useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1,
springDampingAllDirections=1, useSelfCollision=0, frictionCoeff=.5, useFaceContact=1)
p.changeVisualShape(clothId, -1, flags=p.VISUAL_SHAPE_DOUBLE_SIDED)
p.createSoftBodyAnchor(clothId, 24, -1, -1)
p.createSoftBodyAnchor(clothId, 20, -1, -1)
p.createSoftBodyAnchor(clothId, 15, boxId, -1, [0.5, -0.5, 0])
p.createSoftBodyAnchor(clothId, 19, boxId, -1, [-0.5, -0.5, 0])
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
debug = True
if debug:
data = p.getMeshData(clothId, -1, flags=p.MESH_DATA_SIMULATION_MESH)
print("--------------")
print("data=", data)
print(data[0])
print(data[1])
text_uid = []
for i in range(data[0]):
pos = data[1][i]
arm_height = 1
body_width = 1
mod4 = p.loadURDF("cube_small.urdf",
basePosition=[-1.25, 6, 1],
globalScaling=10)
p.changeVisualShape(mod4, -1, rgbaColor=[0, 0, 1, 1])
armId2 = p.loadSoftBody("arm_hori.obj",
basePosition=[
body_length / 2 + arm_width / 2,
-body_width / 2 + arm_width / 2, arm_height / 2
],
scale=0.1,
mass=100.,
useNeoHookean=1,
useBendingSprings=1,
useMassSpring=1,
springElasticStiffness=400,
springDampingStiffness=1,
springDampingAllDirections=1,
useSelfCollision=1,
frictionCoeff=0,
useFaceContact=1)
p.createSoftBodyAnchor(armId2, 7, mod4, -1)
while 1:
#p.applyExternalForce(mod4,-1,[0,0,0],[0,0,0],p.LINK_FRAME)
p.stepSimulation()
sleep(1 / 100.)
clothId = p.loadSoftBody("cloth_z_up.obj",
basePosition=[0, 0, 2],
scale=0.5,
mass=1.,
useNeoHookean=0,
useBendingSprings=1,
useMassSpring=1,
springElasticStiffness=40,
springDampingStiffness=.99,
springDampingAllDirections=1,
useSelfCollision=0,
frictionCoeff=.5,
useFaceContact=1)
p.resetBaseVelocity(clothId, linearVelocity=[0, 0, 1])
p.createSoftBodyAnchor(clothId, 0, -1, -1)
# p.createSoftBodyAnchor(clothId, 1, -1, -1)
# p.createSoftBodyAnchor(clothId, 3, boxId, -1, [0.5, -0.5, 0])
# p.createSoftBodyAnchor(clothId, 2, boxId, -1, [-0.5, -0.5, 0])
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
p.setRealTimeSimulation(1)
t = True
i = 0
while p.isConnected():
p.applyExternalForce(clothId, 3, [0, 0, 1000], [0, 0, 0], p.WORLD_FRAME)
i += 1
p.setGravity(0, 0, gravZ)
viewMatrix = p.computeViewMatrix(cameraEyePosition=[0, 2, 0],
cameraTargetPosition=[0, 0, 0.5],
cameraUpVector=[0, 0, 1])
