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])