def release(self): """ If suction off, detect contact between gripper and objects. If suction on, detect contact between picked object and other objects. """ if self.activated: self.activated = False # Release gripped rigid object (if any). if self.contact_constraint is not None: try: p.removeConstraint(self.contact_constraint) self.contact_constraint = None except: pass self.init_grip_distance = None self.init_grip_item = None # Release gripped deformable object (if any). if self.def_grip_anchors is not None: for anchorId in self.def_grip_anchors: p.removeConstraint(anchorId) self.def_grip_anchors = None self.def_grip_item = None self.def_min_vetex = None self.def_min_distance = None
def _set_attached_objects(self, prev_object): """ This method updates the positions of all attached objects. This is done by calculating the new pose in world coordinate frame and setting the base pose of the attached objects to this new pose. After this the _set_attached_objects method of all attached objects will be called. :param prev_object: The object that called this method, this will be excluded to prevent recursion in the update. """ for obj in self.attachments: if obj in prev_object: continue if self.attachments[obj][2]: # Updates the attachment transformation and contraint if the # attachment is loos, instead of updating the position of all attached objects link_T_object = self._calculate_transform(obj, self.attachments[obj][1]) link_id = self.get_link_id(self.attachments[obj][1]) if self.attachments[obj][1] else -1 self.attachments[obj][0] = link_T_object obj.attachments[self][0] = p.invertTransform(link_T_object[0], link_T_object[1]) p.removeConstraint(self.cids[obj]) cid = p.createConstraint(self.id, link_id, obj.id, -1, p.JOINT_FIXED, [0, 0, 0], link_T_object[0], [0, 0, 0], link_T_object[1]) self.cids[obj] = cid obj.cids[self] = cid else: # Updates the position of all attached objects link_T_object = self.attachments[obj][0] link_name = self.attachments[obj][1] world_T_link = self.get_link_position_and_orientation(link_name) if link_name else self.get_position_and_orientation() world_T_object = p.multiplyTransforms(world_T_link[0], world_T_link[1], link_T_object[0], link_T_object[1]) p.resetBasePositionAndOrientation(obj.id, world_T_object[0], world_T_object[1]) obj._set_attached_objects(prev_object + [self])
def release(self): """Release gripper object, only applied if gripper is 'activated'. If suction off, detect contact between gripper and objects. If suction on, detect contact between picked object and other objects. To handle deformables, simply remove constraints (i.e., anchors). Also reset any relevant variables, e.g., if releasing a rigid, we should reset init_grip values back to None, which will be re-assigned in any subsequent grasps. """ if self.activated: self.activated = False # Release gripped rigid object (if any). if self.contact_constraint is not None: try: p.removeConstraint(self.contact_constraint) self.contact_constraint = None except: # pylint: disable=bare-except pass self.init_grip_distance = None self.init_grip_item = None # Release gripped deformable object (if any). if self.def_grip_anchors: for anchor_id in self.def_grip_anchors: p.removeConstraint(anchor_id) self.def_grip_anchors = [] self.def_grip_item = None self.def_min_vetex = None self.def_min_distance = None
def constrain(obj1, obj2, link, cpos, pos, id_lookup, constraints, ur5_dist): """ Constrain two objects :params: obj1 - object to be constrained obj2 - target object to which obj1 is constrained link - link lookup for objects to be constrained id_lookup - id dictionary to lookup object id by name constraints - current list of constraints ur5_dist - dictionary to lookup distance from ur5 gripper :return: cid - constraint id """ if obj1 in constraints.keys(): p.removeConstraint(constraints[obj1][1]) count = 0 # count checks where to place on target object for obj in constraints.keys(): if constraints[obj][0] == obj2: count += 1 print("New constraint=", obj1, " on ", obj2) # parent is the target, child is the object if obj2 == "ur5": cid = p.createConstraint(id_lookup[obj2], link[obj2], id_lookup[obj1], link[obj1], p.JOINT_POINT2POINT, [0, 0, 0], parentFramePosition=ur5_dist[obj1], childFramePosition=cpos[obj1][0], childFrameOrientation=[0,0,0,0]) else: cid = p.createConstraint(id_lookup[obj2], link[obj2], id_lookup[obj1], link[obj1], p.JOINT_POINT2POINT, [0, 0, 0], parentFramePosition=pos[obj2][count], childFramePosition=cpos[obj1][0], childFrameOrientation=[0,0,0,0]) return cid
def remove_constraint(self, id): """ Remove a constraint """ if id in self.constraint_id_map: p.removeConstraint(self.constraint_id_map[id]) del self.constraint_id_map[id] base_link_sim_id = self.entity_id_map[id] p.changeDynamics(base_link_sim_id, -1, activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
def remove_constraints(self): if (len(self.macroaction.link_status) > 0): for obj1_index in range(len(self.objects)): for obj2_index in range(obj1_index, len(self.objects)): link_index = obj1_index * len(self.objects) + obj2_index if (self.macroaction.links[link_index] is not None): p.removeConstraint(self.macroaction.links[link_index]) self.macroaction.links[link_index] = None
def remove_constraint(self): """ Remove constraints created by create_constraint """ for cid in self.cid: p.removeConstraint(cid) self.cid = [] self.constraint_marker.set_position([0, 0, 100]) self.constraint_marker2.set_position([0, 0, 100])
def simple_simulate(self): for parameter_count in range(1000): indexes = cal_parameter_group(parameter_count) spring_elastic_stiffness = 40.0 spring_damping_stiffness = 0.1 spring_bending_stiffness = 2.0 table_id, cloth_id, anchor_ids = self.load_world( spring_elastic_stiffness, spring_damping_stiffness, spring_bending_stiffness) iteration = 0 iteration_name = "{:.1f}_{:.1f}_{:.1f}".format( spring_elastic_stiffness, spring_damping_stiffness, spring_bending_stiffness) print("Saving file: ", iteration_name) start = time.time() for step in range(1150): # suspend and release the cloth if step == 0: p.resetBaseVelocity(cloth_id, linearVelocity=[0, 0, 0.5]) # p.applyExternalForce(cloth_id, 0, [0, 0, 1000], [0, 0, 0], p.WORLD_FRAME) elif step == 500: p.resetBaseVelocity(cloth_id, linearVelocity=[0, 0, 0]) for anchor_id in anchor_ids: p.removeConstraint(anchor_id) # after releasing the cloth, record the data every 25 frame of simulation if step >= 50 and step % 50 == 0: for camera_position, eye_position in enumerate( self.available_eye_positions)[3]: target_position = p.getBasePositionAndOrientation( cloth_id)[0] view_matrix = p.computeViewMatrix( cameraEyePosition=eye_position, cameraTargetPosition=target_position, cameraUpVector=self.up_vector) projection_matrix = p.computeProjectionMatrixFOV( fov=90, aspect=1.0, nearVal=0.01, farVal=5.1) camera_image = p.getCameraImage( width=self.width, height=self.height, viewMatrix=view_matrix, projectionMatrix=projection_matrix, renderer=self.renderer) file_name = iteration_name + '_' + str( iteration) + '_' + str(camera_position) thread = Thread(target=save, args=(file_name, camera_image)) thread.start() time.sleep( 0.05 ) # force the program sleep to avoid too many threads run at the same time iteration += 1 print("finish saving in ", time.time() - start) p.stepSimulation()
def removeConstraint(constraints, obj1, obj2): """ Remove constraint between two objects :params: constraints - current dictionary of constraints obj1 - constrained object obj2 - target object to which obj1 is constrained """ if obj1 in constraints.keys(): p.removeConstraint(constraints[obj1][1])
def remove_constraint(self, constraint_uid): """Remove a constraint. Args: constraint_uid: The constraint unique ID. """ # TODO(kuanfang): removeConstraint dose not work. pybullet.changeConstraint(constraint_uid, maxForce=0, physicsClientId=self.uid) pybullet.removeConstraint(constraint_uid, physicsClientId=self.uid)
def detach(self, object): """ This method detaches an other object from this object. After the detachment the detachment event of the corresponding BulletWorld w:ill be fired. :param object: The object which should be detached """ if object not in self.attachments: return p.removeConstraint(self.attachments[object][0]) del self.attachments[object] del object.attachments[self] self.world.detachment_event(self, [self, object])
def delete_constraint(self, constraintId): if constraintId in self.constraints: constraint = self.constraints[constraintId] if constraintId in self.constraint_deletion_cbs: for cb in self.constraint_deletion_cbs[constraintId]: cb(self, constraintId, constraint) del self.constraint_deletion_cbs[constraintId] pb.removeConstraint(constraint.bId(), self.__client_id) del self.__cId_IdMap[constraint.bId()] del self.constraints[constraintId] return True return False
def detach(self, object): """ This method detaches an other object from this object. This is done by deleting the attachment from the attachments dictionaty of both obejects and deleting the constraint of pybullet. After the detachment the detachment event of the corresponding BulletWorld w:ill be fired. :param object: The object which should be detached """ del self.attachments[object] del object.attachments[self] p.removeConstraint(self.cids[object], physicsClientId=self.world.client_id) del self.cids[object] del object.cids[self] self.world.detachment_event(self, [self, object])
def undock(self,other): ''' This undocks the two modules that is self->bot and other->bot ''' ind = None try: ind = self.dockees.index(other) except: raise Exception(str(self.id)+" "+str(other.id )+", These two modules have not been docked before") # print(self.id, other.id, "Are not docked before") finally: if ind is not None: cid = self.constraints.pop(ind) other.constraints.remove(cid) p.removeConstraint(cid,physicsClientId=self.pClient) self.dockees.remove(other) other.dockees.remove(self)
def restoreOnInput(world_states, x1, y1, o1, constraints): """ Restore to last saved state when this function is called """ print(world_states) if len(world_states) != 0: world_states.pop() id1, x, y, o, cids_old = world_states[-1] cids_list_old = [] for obj in cids_old.keys(): cids_list_old.append(cids_old[obj][1]) for obj in constraints.keys(): if not constraints[obj][1] in cids_list_old: p.removeConstraint(constraints[obj][1]) del (constraints[obj]) p.restoreState(stateId=id1) # q=p.getQuaternionFromEuler((0,0,0)) # p.resetBasePositionAndOrientation(([0, 0, 0], q)) # Get robot to home when undo # return 0, 0, 0, world_states return x, y, o, constraints, world_states return x1, y1, o1, constraints, world_states
def reset(self): self._setup_ee() self._open_gripper() if self.gripperConstraint: p.removeConstraint(self.gripperConstraint) p.resetBasePositionAndOrientation(self.gripperId, self.initGripperPos, self.endEffectorOrientation) self.gripperConstraint = p.createConstraint( self.gripperId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0], self.initGripperPos, self.endEffectorOrientation) self._close_gripper() # set robot init config self._clear_state_between_control_steps() # start at rest for _ in range(1000): p.stepSimulation() self.state = self._obs() self._dd.draw_point('x0', self.get_ee_pos(self.state), color=(0, 1, 0)) return np.copy(self.state)
def reset(self): # reset robot to nominal pose p.resetBasePositionAndOrientation(self.pusherId, self.initPusherPos, [0, 0, 0, 1]) p.resetBasePositionAndOrientation( self.blockId, self.initBlockPos, p.getQuaternionFromEuler([0, 0, self.initBlockYaw])) # set robot init config if self.pusherConstraint: p.removeConstraint(self.pusherConstraint) self.pusherConstraint = p.createConstraint(self.pusherId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0], self.initPusherPos) self._clear_state_between_control_steps() # start at rest for _ in range(1000): p.stepSimulation() self.state = self._obs() self._dd.draw_2d_pose('x0', self.get_block_pose(self.state), color=(0, 1, 0)) return np.copy(self.state)
def loadRobot(self, translation, quaternion, physicsClientId=0): """ Overloads @loadRobot from the @RobotVirtual class, loads the robot into the simulated instance. This method also updates the max velocity of the robot's fingers, adds self collision filters to the model and defines the cameras of the model in the camera_dict. Parameters: translation - List containing 3 elements, the translation [x, y, z] of the robot in the WORLD frame quaternion - List containing 4 elements, the quaternion [x, y, z, q] of the robot in the WORLD frame physicsClientId - The id of the simulated instance in which the robot is supposed to be loaded """ pybullet.setAdditionalSearchPath(os.path.dirname( os.path.realpath(__file__)), physicsClientId=physicsClientId) # Add 0.50 meters on the z component, avoing to spawn NAO in the ground translation = [translation[0], translation[1], translation[2] + 1.05] RobotVirtual.loadRobot(self, translation, quaternion, physicsClientId=physicsClientId) balance_constraint = pybullet.createConstraint( parentBodyUniqueId=self.robot_model, parentLinkIndex=-1, childBodyUniqueId=-1, childLinkIndex=-1, jointType=pybullet.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=[0, 0, 0], parentFrameOrientation=[0, 0, 0, 1], childFramePosition=translation, childFrameOrientation=quaternion, physicsClientId=self.physics_client) self.goToPosture("Stand", 1.0) pybullet.setCollisionFilterPair(self.robot_model, self.robot_model, self.link_dict["REye"].getIndex(), self.link_dict["LEye"].getIndex(), 0, physicsClientId=self.physics_client) for link in ["torso", "HeadRoll_link"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict["NeckPitch_link"].getIndex(), self.link_dict[link].getIndex(), 0, physicsClientId=self.physics_client) for side in ["R", "L"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Eye"].getIndex(), self.link_dict["HeadRoll_link"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Thigh"].getIndex(), self.link_dict["body"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side.lower() + "_ankle"].getIndex(), self.link_dict[side + "Tibia"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "ShoulderYaw_link"].getIndex(), self.link_dict["torso"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "WristRoll_link"].getIndex(), self.link_dict[side.lower() + "_wrist"].getIndex(), 0, physicsClientId=self.physics_client) for link in ["ShoulderYaw_link", "WristYaw_link"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + link].getIndex(), self.link_dict[side + "Elbow"].getIndex(), 0, physicsClientId=self.physics_client) for name, link in self.link_dict.items(): if side + "Finger" in name or\ side + "Thumb" in name: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side.lower() + "_wrist"].getIndex(), link.getIndex(), 0, physicsClientId=self.physics_client) for joint in self.joint_dict.values(): pybullet.resetJointState(self.robot_model, joint.getIndex(), 0.0, physicsClientId=self.physics_client) pybullet.removeConstraint(balance_constraint, physicsClientId=self.physics_client) for joint_name in list(self.joint_dict): if 'RFinger' in joint_name or 'RThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["RHand"].getMaxVelocity()) elif 'LFinger' in joint_name or 'LThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["LHand"].getMaxVelocity()) camera_right = CameraRgb( self.robot_model, RomeoVirtual.ID_CAMERA_RIGHT, self.link_dict["CameraRightEye_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) camera_left = CameraRgb(self.robot_model, RomeoVirtual.ID_CAMERA_LEFT, self.link_dict["CameraLeftEye_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) camera_depth = CameraDepth(self.robot_model, RomeoVirtual.ID_CAMERA_DEPTH, self.link_dict["CameraDepth_optical_frame"], hfov=58.0, vfov=45.0, physicsClientId=self.physics_client) self.camera_dict = { RomeoVirtual.ID_CAMERA_RIGHT: camera_right, RomeoVirtual.ID_CAMERA_LEFT: camera_left, RomeoVirtual.ID_CAMERA_DEPTH: camera_depth } # The frequency of the IMU is set to 100Hz self.imu = Imu(self.robot_model, self.link_dict["torso"], 100.0, physicsClientId=self.physics_client)
def remove_all_constraints(): for cid in get_constraint_ids(): p.removeConstraint(cid)
def release_hold(self, armname): p.removeConstraint(self.grasped[armname])
def remove_suction_force(self, cons): p.removeConstraint(cons) for i in range(10): p.stepSimulation()
POSITION = 1 ORIENTATION = 2 BUTTONS = 6 p.setRealTimeSimulation(1) controllerId = -1 while True: events = p.getVREvents() for e in (events): #print (e[BUTTONS]) if (e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED): if (controllerId >= 0): controllerId = -1 else: controllerId = e[0] if (e[0] == controllerId): if (robot_cid >= 0): p.changeConstraint(robot_cid, e[POSITION], e[ORIENTATION], maxForce=5000) if (e[BUTTONS][32] & p.VR_BUTTON_WAS_TRIGGERED): if (robot_cid >= 0): p.removeConstraint(robot_cid) #q = [0,0,0,1] euler = p.getEulerFromQuaternion(e[ORIENTATION]) q = p.getQuaternionFromEuler([euler[0], euler[1], 0]) #-euler[0],-euler[1],-euler[2]]) robot_cid = p.createConstraint(minitaur, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.1, 0, 0], e[POSITION], q, e[ORIENTATION])
def remove_constraint(constraint): p.removeConstraint(constraint, physicsClientId=CLIENT)
def remove_constraint(self, id): if id not in self.constraint_id_map: raise ValueError( "Constraint for entity <{}> not created".format(id)) p.removeConstraint(self.constraint_id[id])
def simulate(self): """ simulate the environment using pybullet and save file :return: """ for speed in np.arange(5, 10): for rand in range(10): for spring_elastic_stiffness in self.elastic_stiffness_range: spring_damping_stiffness = 0.1 spring_bending_stiffness = 2.0 table_id, cloth_id, anchor_ids = self.load_world( spring_elastic_stiffness, spring_damping_stiffness, spring_bending_stiffness, rand_pos=True) iteration = 0 iteration_name = "{:.1f}_{:.1f}_{:.1f}".format( spring_elastic_stiffness, spring_damping_stiffness, spring_bending_stiffness) print("Saving file: ", iteration_name, ', speed: ', speed, ', rand: ', rand) start = time.time() for step in range(1000): # suspend and release the cloth if step == 0: # p.createSoftBodyAnchor(cloth_id, 1, -1, -1) p.resetBaseVelocity( cloth_id, linearVelocity=[0, 0, speed / 10]) # p.applyExternalForce(cloth_id, 1, [0, 0, 1000], [0, 0, 100], p.WORLD_FRAME) elif step == 450: p.resetBaseVelocity(cloth_id, linearVelocity=[0, 0, 0]) for anchor_id in anchor_ids: p.removeConstraint(anchor_id) # after releasing the cloth, record the data every 25 frame of simulation if step > 500 and step % 20 == 0: for camera_position, eye_position in enumerate( self.available_eye_positions): target_position = [ 0.0, -0.1, 0.5 ] # p.getBasePositionAndOrientation(cloth_id)[0] view_matrix = p.computeViewMatrix( cameraEyePosition=eye_position, cameraTargetPosition=target_position, cameraUpVector=self.up_vector) projection_matrix = p.computeProjectionMatrixFOV( fov=45.0, aspect=1.0, nearVal=0.01, farVal=5.1) # p.computeViewMatrixFromYawPitchRoll() camera_image = p.getCameraImage( width=self.width, height=self.height, viewMatrix=view_matrix, projectionMatrix=projection_matrix, renderer=self.renderer) file_name = '{}_{}_{}_{}_{}'.format( iteration_name, iteration, camera_position, speed, rand) thread = Thread(target=save, args=(file_name, camera_image)) thread.start() time.sleep( 0.05 ) # force the program sleep to avoid too many threads run at the same time iteration += 1 print("finish saving in ", time.time() - start) p.stepSimulation()
def remove(self): pb.removeConstraint(self._constraint_id, physicsClientId=self.client_id)
def deconstrain(self): if (not self.training): p.removeConstraint(self.knot)
import pybullet as p import time import math p.connect(p.GUI) p.loadURDF("plane.urdf") cubeId = p.loadURDF("cube_small.urdf",0,0,1) p.setGravity(0,0,-10) p.setRealTimeSimulation(1) cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1]) print (cid) print (p.getConstraintUniqueId(0)) prev=[0,0,1] a=-math.pi while 1: a=a+0.01 if (a>math.pi): a=-math.pi time.sleep(.01) p.setGravity(0,0,-10) pivot=[a,0,1] orn = p.getQuaternionFromEuler([a,0,0]) p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50) p.removeConstraint(cid)
p.setRealTimeSimulation(1) controllerId = -1 while True: events = p.getVREvents() for e in (events): #print (e[BUTTONS]) if (e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED): if (controllerId >= 0): controllerId = -1 else: controllerId = e[0] if (e[0] == controllerId): if (robot_cid >= 0): p.changeConstraint(robot_cid, e[POSITION], e[ORIENTATION], maxForce=5000) if (e[BUTTONS][32] & p.VR_BUTTON_WAS_TRIGGERED): if (robot_cid >= 0): p.removeConstraint(robot_cid) #q = [0,0,0,1] euler = p.getEulerFromQuaternion(e[ORIENTATION]) q = p.getQuaternionFromEuler([euler[0], euler[1], 0]) #-euler[0],-euler[1],-euler[2]]) robot_cid = p.createConstraint(minitaur, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.1, 0, 0], e[POSITION], q, e[ORIENTATION])
def loadRobot(self, translation, quaternion, physicsClientId=0): """ Overloads @loadRobot from the @RobotVirtual class, loads the robot into the simulated instance. This method also updates the max velocity of the robot's fingers, adds self collision filters to the model and defines the cameras of the model in the camera_dict. Parameters: translation - List containing 3 elements, the translation [x, y, z] of the robot in the WORLD frame quaternion - List containing 4 elements, the quaternion [x, y, z, q] of the robot in the WORLD frame physicsClientId - The id of the simulated instance in which the robot is supposed to be loaded """ pybullet.setAdditionalSearchPath( os.path.dirname(os.path.realpath(__file__)), physicsClientId=physicsClientId) # Add 0.36 meters on the z component, avoing to spawn NAO in the ground translation = [translation[0], translation[1], translation[2] + 0.36] RobotVirtual.loadRobot( self, translation, quaternion, physicsClientId=physicsClientId) balance_constraint = pybullet.createConstraint( parentBodyUniqueId=self.robot_model, parentLinkIndex=-1, childBodyUniqueId=-1, childLinkIndex=-1, jointType=pybullet.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=[0, 0, 0], parentFrameOrientation=[0, 0, 0, 1], childFramePosition=translation, childFrameOrientation=quaternion, physicsClientId=self.physics_client) self.goToPosture("Stand", 1.0) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict["torso"].getIndex(), self.link_dict["Head"].getIndex(), 0, physicsClientId=self.physics_client) for side in ["R", "L"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Thigh"].getIndex(), self.link_dict[side + "Hip"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Bicep"].getIndex(), self.link_dict[side + "ForeArm"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Pelvis"].getIndex(), self.link_dict[side + "Thigh"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Tibia"].getIndex(), self.link_dict[side.lower() + "_ankle"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Finger11_link"].getIndex(), self.link_dict[side + "Finger13_link"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Finger21_link"].getIndex(), self.link_dict[side + "Finger23_link"].getIndex(), 0, physicsClientId=self.physics_client) for base_link in ["RThigh", "LThigh", "RBicep", "LBicep"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict["torso"].getIndex(), self.link_dict[base_link].getIndex(), 0, physicsClientId=self.physics_client) for name, link in self.link_dict.items(): for wrist in ["r_wrist", "l_wrist"]: if wrist[0] + "finger" in name.lower() or\ wrist[0] + "thumb" in name.lower(): pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[wrist].getIndex(), link.getIndex(), 0, physicsClientId=self.physics_client) for joint in self.joint_dict.values(): pybullet.resetJointState( self.robot_model, joint.getIndex(), 0.0, physicsClientId=self.physics_client) pybullet.removeConstraint( balance_constraint, physicsClientId=self.physics_client) for joint_name in list(self.joint_dict): if 'RFinger' in joint_name or 'RThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["RHand"].getMaxVelocity()) elif 'LFinger' in joint_name or 'LThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["LHand"].getMaxVelocity()) camera_top = CameraRgb( self.robot_model, NaoVirtual.ID_CAMERA_TOP, self.link_dict["CameraTop_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) camera_bottom = CameraRgb( self.robot_model, NaoVirtual.ID_CAMERA_BOTTOM, self.link_dict["CameraBottom_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) self.camera_dict = { NaoVirtual.ID_CAMERA_TOP: camera_top, NaoVirtual.ID_CAMERA_BOTTOM: camera_bottom} # The frequency of the IMU is set to 100Hz self.imu = Imu( self.robot_model, self.link_dict["torso"], 100.0, physicsClientId=self.physics_client) # FSRs fsr_dict = dict() for name in NaoFsr.LFOOT + NaoFsr.RFOOT: fsr_dict[name] = Fsr( self.robot_model, self.link_dict[name], physicsClientId=self.physics_client) self._setFsrHandler(FsrHandler(fsr_dict))
import pybullet as p import time import math from pbsbtest.utils import res_pkg_path p.connect(p.GUI) p.loadURDF(res_pkg_path("package://pbsbtest/urdf/plane.urdf")) cubeId = p.loadURDF(res_pkg_path("package://pbsbtest/urdf/cube_small.urdf"), 0, 0, 1) p.setGravity(0, 0, -10) p.setRealTimeSimulation(1) cid = p.createConstraint(cubeId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1]) print cid print p.getConstraintUniqueId(0) prev = [0, 0, 1] a = -math.pi while 1: a = a + 0.01 if (a > math.pi): a = -math.pi time.sleep(.01) p.setGravity(0, 0, -10) pivot = [a, 0, 1] orn = p.getQuaternionFromEuler([a, 0, 0]) p.changeConstraint(cid, pivot, jointChildFrameOrientation=orn, maxForce=50) p.removeConstraint(cid)
def loadRobot(self, translation, quaternion, physicsClientId=0): """ Overloads @loadRobot from the @RobotVirtual class. Update max velocity for the fingers and thumbs, based on the hand joints. Add self collision exceptions. Add the cameras. """ pybullet.setAdditionalSearchPath(os.path.dirname( os.path.realpath(__file__)), physicsClientId=physicsClientId) # Add 0.36 meters on the z component, avoing to spawn NAO in the ground translation = [translation[0], translation[1], translation[2] + 0.36] RobotVirtual.loadRobot(self, translation, quaternion, physicsClientId=physicsClientId) balance_constraint = pybullet.createConstraint( parentBodyUniqueId=self.robot_model, parentLinkIndex=-1, childBodyUniqueId=-1, childLinkIndex=-1, jointType=pybullet.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=[0, 0, 0], parentFrameOrientation=[0, 0, 0, 1], childFramePosition=translation, childFrameOrientation=quaternion, physicsClientId=self.physics_client) self.goToPosture("Stand", 1.0) pybullet.setCollisionFilterPair(self.robot_model, self.robot_model, self.link_dict["torso"].getIndex(), self.link_dict["Head"].getIndex(), 0, physicsClientId=self.physics_client) for side in ["R", "L"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Thigh"].getIndex(), self.link_dict[side + "Hip"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Bicep"].getIndex(), self.link_dict[side + "ForeArm"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Pelvis"].getIndex(), self.link_dict[side + "Thigh"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Tibia"].getIndex(), self.link_dict[side.lower() + "_ankle"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Finger11_link"].getIndex(), self.link_dict[side + "Finger13_link"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Finger21_link"].getIndex(), self.link_dict[side + "Finger23_link"].getIndex(), 0, physicsClientId=self.physics_client) for base_link in ["RThigh", "LThigh", "RBicep", "LBicep"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict["torso"].getIndex(), self.link_dict[base_link].getIndex(), 0, physicsClientId=self.physics_client) for name, link in self.link_dict.items(): for wrist in ["r_wrist", "l_wrist"]: if wrist[0] + "finger" in name.lower() or\ wrist[0] + "thumb" in name.lower(): pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[wrist].getIndex(), link.getIndex(), 0, physicsClientId=self.physics_client) pybullet.removeConstraint(balance_constraint, physicsClientId=self.physics_client) for joint_name in list(self.joint_dict): if 'RFinger' in joint_name or 'RThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["RHand"].getMaxVelocity()) elif 'LFinger' in joint_name or 'LThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["LHand"].getMaxVelocity()) self.camera_top = CameraRgb(self.robot_model, self.link_dict["CameraTop_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) self.camera_bottom = CameraRgb( self.robot_model, self.link_dict["CameraBottom_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client)