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 Reset(self, reload_urdf=True): """Reset the minitaur to its initial states. Args: reload_urdf: Whether to reload the urdf file. If not, Reset() just place the minitaur back to its starting position. """ if reload_urdf: if self._self_collision_enabled: self.quadruped = pybullet.loadURDF( "quadruped/minitaur.urdf", INIT_POSITION, flags=pybullet.URDF_USE_SELF_COLLISION) else: self.quadruped = pybullet.loadURDF("quadruped/minitaur.urdf", INIT_POSITION) self._BuildJointNameToIdDict() self._BuildMotorIdList() self._RecordMassInfoFromURDF() self.ResetPose(add_constraint=True) if self._on_rack: pybullet.createConstraint(self.quadruped, -1, -1, -1, pybullet.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1]) else: pybullet.resetBasePositionAndOrientation(self.quadruped, INIT_POSITION, INIT_ORIENTATION) pybullet.resetBaseVelocity(self.quadruped, [0, 0, 0], [0, 0, 0]) self.ResetPose(add_constraint=False) self._overheat_counter = np.zeros(self.num_motors) self._motor_enabled_list = [True] * self.num_motors
def __init__(self, robot, tool): self.robot = robot self.tool = tool pos = [0.487, 0.109, 0.421] rot = p.getQuaternionFromEuler([np.pi, 0, 0]) urdf = 'assets/ur5/gripper/robotiq_2f_85.urdf' self.body = p.loadURDF(urdf, pos, rot) self.n_joints = p.getNumJoints(self.body) self.activated = False # Connect gripper base to robot tool p.createConstraint(self.robot, tool, self.body, 0, jointType=p.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, -0.05]) # Set friction coefficients for gripper fingers for i in range(p.getNumJoints(self.body)): p.changeDynamics( self.body, i, lateralFriction=1.5, spinningFriction=1.0, rollingFriction=0.0001, # rollingFriction=1.0, frictionAnchor=True ) # contactStiffness=0.0, contactDamping=0.0 # Start thread to handle additional gripper constraints self.motor_joint = 1
def main(): # Open GUI and set pybullet_data in the path p.connect(p.GUI) #p.setAdditionalSearchPath(pybullet_data.getDataPath()) # Load plane contained in pybullet_data p.loadURDF(os.path.join(pybullet_data.getDataPath(),"plane.urdf")) # Set gravity for simulation p.setGravity(0,0,-9.8) # load robot model robotIds = p.loadSDF(os.path.join(robot_data.getDataPath(), "iCub/icub_fixed_model.sdf")) icubId = robotIds[0] # set constraint between base_link and world p.createConstraint(icubId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0], [p.getBasePositionAndOrientation(icubId)[0][0], p.getBasePositionAndOrientation(icubId)[0][1], p.getBasePositionAndOrientation(icubId)[0][2]*1.2], p.getBasePositionAndOrientation(icubId)[1]) ##init_pos for standing # without FT_sensors init_pos = [0]*15 + [-29.4, 28.8, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 44.59, 0, 0, 0] # with FT_sensors #init_pos = [0]*19 + [-29.4, 28.8, 0, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 0, 44.59, 0, 0, 0] # all set to zero #init_pos = [0]*p.getNumJoints(icubId) # Load other objects p.loadURDF(os.path.join(pybullet_data.getDataPath(),"table/table.urdf"), [1.1, 0.0, 0.0]) p.loadURDF(os.path.join(pybullet_data.getDataPath(), "lego/lego.urdf"), [0.5,0.0,0.8]) # add debug slider jointIds=[] paramIds=[] joints_num = p.getNumJoints(icubId) print("len init_pos ",len(init_pos)) print("len num joints", joints_num) for j in range (joints_num): info = p.getJointInfo(icubId,j) jointName = info[1] #jointType = info[2] jointIds.append(j) paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), info[8], info[9], init_pos[j]/180*m.pi)) while True: for i in range(joints_num): p.setJointMotorControl2(icubId, i, p.POSITION_CONTROL, targetPosition=p.readUserDebugParameter(i), targetVelocity=0.0, positionGain=0.25, velocityGain=0.75, force=50) p.stepSimulation() time.sleep(0.01)
def __init__(self, n_joints, pos, radius, parent=None): ''' Create a pybullet tentacle ''' mass = 1 visualShapeId = -1 self.particles = [None] * n_joints self.radius = radius if parent is None: base_shape = p.createCollisionShape(p.GEOM_PLANE) self.base = p.createMultiBody(0, base_shape, -1, pos) else: self.base = parent colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=self.radius) p.setCollisionFilterGroupMask(self.base,-1,0,0) for i in range (n_joints): self.particles[i] = p.createMultiBody(mass, colSphereId ,visualShapeId, [pos[0], pos[1], pos[2] + i*2*self.radius + self.radius]) if i > 0: p.createConstraint(self.particles[i-1], -1, self.particles[i], -1, p.JOINT_FIXED,[0,0,0], [0,0, 2*self.radius], [0,0,0]) else: p.setCollisionFilterPair(self.base, self.particles[i], -1, -1, True) p.createConstraint(self.base, -1, self.particles[i], -1, p.JOINT_FIXED, [0,0,0], [0,0,self.radius], [0,0,0], [0,0,0,1]) p.changeDynamics(self.particles[i],-1, mass=1, linearDamping=1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
def attach( self, new_robot, link_name, position=(0, 0, 0), orientation=(0, 0, 0, 1) ): """ Attach a new robot (`new_robot`) to self by creating a fixed joint between a specific link (`link_name`) and the base of the new robot. """ assert isinstance(new_robot, Robot) link_idx = self.get_joint_index_by_name(link_name) link_pos = self.get_link_state(link_idx).link_world_position link_pos = np.array(link_pos) + np.array(position) new_robot.set_base_pose(link_pos) p.createConstraint( parentBodyUniqueId=self.id, parentLinkIndex=link_idx, childBodyUniqueId=new_robot.id, childLinkIndex=-1, jointType=p.JOINT_FIXED, jointAxis=[0, 0, 0], # ignored for JOINT_FIXED parentFramePosition=position, childFramePosition=[0, 0, 0], parentFrameOrientation=orientation, childFrameOrientation=[0, 0, 0, 1], **self._client_kwargs, )
def addClosedChainConstraint(self, linkName, linkFrame2Joint): linkState = p.getLinkState(self.cliffordID, self.linkNameToID[linkName], physicsClientId=self.physicsClientId) bodyState = p.getBasePositionAndOrientation( self.cliffordID, physicsClientId=self.physicsClientId) world2joint = p.multiplyTransforms(linkState[4], linkState[5], linkFrame2Joint, [0, 0, 0, 1]) body2world = p.invertTransform(bodyState[0], bodyState[1]) body2joint = p.multiplyTransforms(body2world[0], body2world[1], world2joint[0], world2joint[1]) linkcm2world = p.invertTransform(linkState[0], linkState[1]) linkcm2joint = p.multiplyTransforms(linkcm2world[0], linkcm2world[1], world2joint[0], world2joint[1]) c = p.createConstraint(self.cliffordID, -1, self.cliffordID, self.linkNameToID[linkName], p.JOINT_POINT2POINT, [0, 0, 0], body2joint[0], linkcm2joint[0], physicsClientId=self.physicsClientId) c = p.createConstraint(self.cliffordID, self.jointNameToID['axle2frwheel'], self.cliffordID, self.jointNameToID['axle2flwheel'], jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0], physicsClientId=self.physicsClientId) p.changeConstraint(c, gearRatio=-1, maxForce=10000, physicsClientId=self.physicsClientId)
def __init__(self, config, parent=None): Receptor.__init__(self) self.name = config.name self.position = config.get('xyz', [0., 0., 0.]) self.orientation = p.getQuaternionFromEuler(config.get('rpy', [0., 0., 0.])) use_fixed_base = config.get('use_fixed_base', False) scale = config.get('scale', 1.0) urdf = config.get('model') try: full_urdf_path = next( os.path.join(path, urdf) for path in urdf_path if os.path.isfile(os.path.join(path, urdf))) except StopIteration: raise ValueError('Could not find URDF: {urdf} in {urdf_path}') flags = 0 if config.get('URDF_USE_MATERIAL_COLORS_FROM_MTL', False): flags = flags | p.URDF_USE_MATERIAL_COLORS_FROM_MTL if config.get('URDF_USE_SELF_COLLISION_EXCLUDE_PARENT', False): flags = flags | p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT flags = flags | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS flags = flags | p.URDF_USE_SELF_COLLISION if config.get('URDF_USE_IMPLICIT_CYLINDER', False): flags |= p.URDF_USE_IMPLICIT_CYLINDER if config.get('URDF_MERGE_FIXED_LINKS', False): flags |= p.URDF_MERGE_FIXED_LINKS self.uid = p.loadURDF(full_urdf_path, useFixedBase=use_fixed_base, globalScaling=scale, flags=flags) if parent is None: p.resetBasePositionAndOrientation(self.uid, self.position, self.orientation) else: parent_frame_id = parent.get_frame_id(config.get('parent_frame')) if 'parent_frame' in config else -1 child_frame_id = self.get_frame_id(config.get('child_frame')) if 'child_frame' in config else -1 pose = p.getLinkState(parent.uid, parent_frame_id)[:2] if parent_frame_id != -1 else p.getBasePositionAndOrientation(parent.uid)[:2] p.resetBasePositionAndOrientation(self.uid, *pose) p.createConstraint(parent.uid, parent_frame_id, self.uid, child_frame_id, p.JOINT_FIXED, [0, 0, 1], self.position, [0, 0, 0], self.orientation, [0, 0, 0, 1]) if 'mass' in config: p.changeDynamics(self.uid, -1, mass=config.get('mass')) if 'color' in config: p.changeVisualShape(self.uid, -1, rgbaColor=config.get('color')) self.addons = OrderedDict( sorted( {child.name: AddonFactory.build(child.get('addon'), self, child) for child in config.find_all('addon')}.items(), key=lambda t: t[0])) self.models = OrderedDict( sorted({child.name: Model(child, self) for child in config.find_all('model')}.items(), key=lambda t: t[0]))
def __init__(self, pos=None, orn=None, fixed_height=None): # Load the robot if pos is None: pos = [0.0, 0, 0.40] if orn is None: orn = pybullet.getQuaternionFromEuler([0, 0, 0]) pybullet.setAdditionalSearchPath(TeststandConfig.paths["package"]) self.urdf_path = TeststandConfig.urdf_path self.robotId = pybullet.loadURDF( self.urdf_path, pos, orn, flags=pybullet.URDF_USE_INERTIA_FROM_FILE, useFixedBase=False, ) self.pin_robot = TeststandConfig.buildRobotWrapper() # Query all the joints. num_joints = pybullet.getNumJoints(self.robotId) for ji in range(num_joints): pybullet.changeDynamics( self.robotId, ji, linearDamping=0.04, angularDamping=0.04, restitution=0.0, lateralFriction=0.5, ) self.base_link_name = "base_link" controlled_joints = ["joint_z", "HFE", "KFE"] self.joint_names = controlled_joints # Creates the wrapper by calling the super.__init__. super(TeststandRobot, self).__init__(self.robotId, self.pin_robot, controlled_joints, ["END"], useFixedBase=True) self.nb_dof = self.nv if fixed_height: pybullet.createConstraint( self.robotId, 0, -1, -1, pybullet.JOINT_FIXED, [0, 0, 0], [0, 0, 0.0], [0, 0, fixed_height], )
def __init__(self,cap=cv2.VideoCapture(0),mod="Direct",epsilon=150,preprocess=None,resolution=(56,56,3)): self.cap = cap if mod == "GUI": self.clientId = p.connect(p.GUI) ## This is just to see the hand through opengl window so hence set this as you see the hand as you want to see else: self.clientId = p.connect(p.DIRECT) #p.setRealTimeSimulation(1,physicsClientId=self.clientId) p.resetDebugVisualizerCamera(cameraDistance=2, cameraYaw=0, cameraPitch=-40, cameraTargetPosition=[0,0,2],physicsClientId=self.clientId) self.action_space = spaces.Box(low=np.array([0]*10+[-0.52,-1.04]) ,high=np.array([1.55]*10+[0.52,1.04])) ## down and up (thumb, index, middle, ring, little) , wrist, elbow if len(resolution)!=3: raise Exception("Only a ndim n=3 image can be given as a input") self.res=resolution self.observation_space = spaces.Box(0,2.55,shape=(self.res[0],self.res[1]*2,self.res[2])) p.setAdditionalSearchPath(pybullet_data.getDataPath()) self.plane = p.loadURDF( "plane.urdf" , physicsClientId=self.clientId) p.setAdditionalSearchPath(os.path.abspath("Simulation")) self.handid = p.loadURDF(currentdir+"/hand.urdf",physicsClientId=self.clientId) for i in range(p.getNumJoints(self.handid,physicsClientId=self.clientId)): print(p.getJointInfo(bodyUniqueId=self.handid,jointIndex=i,physicsClientId=self.clientId)) if preprocess is None: self.hand_thresh=self.handmask else: self.hand_thresh=preprocess self.epsilon=epsilon ## Find a good one and set as default self.seed(int(time.time())) ## THis is to match up the no of pixels of our PHATTTT p.createConstraint( self.handid, -1, -1, -1, p.JOINT_FIXED, (0, 0, 1), (0, 0, 0), (0, 0, 0), physicsClientId=self.clientId ) finger_joint_indices = ( (2, 3), # thumb (4, 5), # index (6, 7), # middle (8, 9), # ring (10, 11), # little ) self.hand = robo_hand(self.handid,finger_joint_indices,clientId=self.clientId) self.resetState = p.saveState() self.reset()
def create(self): p.connect(p.GUI) p.createMultiBody(0, 0) p.setGravity(0, 0, -9.8) p.setAdditionalSearchPath(data.getDataPath()) p.loadSDF("stadium.sdf") #p.setRealTimeSimulation(True) #bulid a plate self.plateId = p.createCollisionShape( p.GEOM_BOX, halfExtents=[self.plateLong, self.plateWidth, self.plateHigh]) #bulid a motor self.motorId = p.createCollisionShape(p.GEOM_CYLINDER, radius=self.motorRadius, height=self.motorHeight) #bulid a ball self.ballId = p.createCollisionShape(p.GEOM_SPHERE, radius=self.ballRadius, height=self.motorHeight) self.ballUid = p.createMultiBody(self.ballMass, self.ballId, self.visualShapeId, self.ballStartPos, self.ballStartOrn) #bulit a multi-body self.linkCollisionShapeIndices = [self.motorId] self.controlerUid = p.createMultiBody( self.mass, self.plateId, self.visualShapeId, self.basePosition, self.baseOrientation, linkMasses=self.link_Masses, linkCollisionShapeIndices=self.linkCollisionShapeIndices, linkVisualShapeIndices=self.linkVisualShapeIndices, linkPositions=self.linkPositions, linkOrientations=self.linkOrientations, linkInertialFramePositions=self.linkInertialFramePositions, linkInertialFrameOrientations=self.linkInertialFrameOrientations, linkParentIndices=self.indices, linkJointTypes=self.jointTypes, linkJointAxis=self.axis) #change some physical parameters p.changeDynamics(self.controlerUid, -1, spinningFriction=0.01, rollingFriction=0.01, linearDamping=0.0) #constraint objects #p.createConstraint(self.motorId,-1,-1,-1,p.JOINT_POINT2POINT,[0,0,0],[0,0,0],[0,0,1.5]) p.createConstraint(self.controlerUid, -1, -1, -1, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0, 0], [0, 0, 1.5]) p.setRealTimeSimulation(self.useRealTimeSim)
def __init__(self, sensor_name, robot_id=None, robot_link=None, position_offset=[0, 0, 0], orientation_offset=[0, 0, 0, 1], fixed_pose=True, urdf_file=None, urdf_flags=0): ''' Args: sensor_name (str): Identifier for this sensor Optional Args: robot_id (int): PyBullet id of robot this belongs to robot_link (int): Link id where this joint is 'mounted' position_offset (Tuple of 3 floats): X,Y,Z position of sensor relative to mount point (or world frame if robot_id is None) orientation_offset (Tuple of 4 floats): Quaternion orientation of sensor relative to mount point (or world frame if robot_id is None) fixed_pose (Bool): True if fixed in place, False if movable/subject to physics urdf_file (str): Location of the sensor's URDF file to render urdf_flags (int): PyBullet URDF flags to load with the model ''' self._name = sensor_name self._simulator_id = None self._robot_id = robot_id self._robot_link = robot_link self._position_offset = np.array( position_offset ) # Initial position, could change over the course of simulation! self._orientation_offset = np.array( orientation_offset ) # Initial orientation, could change over the course of simulation! self._debug_mode = False # Set to True to trigger a Sensor's debugging mode (e.g., rendering lasers, line of sight, etc.) # If there's a physical model to render to represent this sensor, we have to care about physics if urdf_file is not None: self._simulator_id = p.loadURDF(urdf_file, basePosition=position_offset, baseOrientation=orientation_offset, useFixedBase=fixed_pose, flags=urdf_flags) if self._robot_id is None: # Sensor exists independent of a robot p.createConstraint(self._simulator_id, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], position_offset, orientation_offset) else: # Sensor is part of the robot, attach it to robot_id on its robot_link p.createConstraint(self._simulator_id, -1, self._robot_id, self._robot_link, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], position_offset, orientation_offset)
def place(self, pos, rot, joints): pb.resetBasePositionAndOrientation(self.handle, pos, rot) pb.createConstraint(self.handle, -1, -1, -1, pb.JOINT_FIXED, pos, [0, 0, 0], rot) for i, q in enumerate(joints): pb.resetJointState(self.handle, i, q) # gripper state for joint in self.gripper_indices: pb.resetJointState(self.handle, joint, 0.) # send commands self.arm(joints, ) self.gripper(self.gripperOpenCommand())
def setup(self, dims=None): #create constraint and a second cup self.cupStartPos = (0, -0.6, 0) self.cupStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) #pick random cup if dims is None: self.cup_name = np.random.choice(self.cup_to_dims.keys()) else: found_cup = False for name in self.cup_to_dims.keys(): if self.cup_to_dims[name][0] == dims[0] and self.cup_to_dims[ name][1] == dims[1]: found_cup = True self.cup_name = name assert (found_cup) cup_file = "urdf/cup/" + self.cup_name self.target_cup = p.loadURDF(cup_file, self.cupStartPos, self.cupStartOrientation, globalScaling=k * 5) self.base_world.drop_beads_in_cup() self.cid = p.createConstraint(self.base_world.cupID, -1, -1, -1, p.JOINT_FIXED, self.cupStartPos, self.cupStartOrientation, [0, 0, 1]) self.bullet_id = p.saveState()
def attach(self, object, parent_link=None, child_link=None): """ This method attaches two objects. This will be done by creating a virtual fixed joint between the two objects. After the attachment the attachment event of the BulletWorld will be fired. It can only exist one attachment between two objects. :param object: The object which should be attached to this object :param parent_link: The Name of the link of this object to which the other object should be attached or -1 for the base position :param child_link: The link Name of the other object to which this object should be attached or -1 for the base """ if object in self.attachments: return parent_link_id = -1 if parent_link is None else self.links[parent_link] child_link_id = -1 if child_link is None else object.links[child_link] world_gripper = p.getLinkState( self.id, parent_link_id )[4] if parent_link_id != -1 else self.get_position() world_object = object.get_position() gripper_object = p.multiplyTransforms( p.invertTransform(world_gripper, [0, 0, 0, 1])[0], [0, 0, 0, 1], world_object, [0, 0, 0, 1], self.world.client_id)[0] cid = p.createConstraint(self.id, parent_link_id, object.id, child_link_id, p.JOINT_FIXED, [0, 1, 0], gripper_object, [0, 0, 0], physicsClientId=self.world.client_id) p.changeConstraint(cid, maxForce=30) self.attachments[object] = cid, parent_link_id object.attachments[self] = cid, parent_link_id self.world.attachment_event(self, [self, object])
def activateNailgun(nailGunID, object1ID, parentRefCOM, childRefCOM, childFrameOrn=[0, 0, 0]): #check to see if they're touching gunContact = p.getContactPoints(nailGunID, object1ID) if not len(gunContact): print('No Contact') return False #get rid of the nasty contact points-just use COM of nailgun + some z component to get pos, and just use straight up as orn #use ray tracing to determine locations of constrain gunPos, _ = p.getBasePositionAndOrientation(nailGunID) rayPos = np.array(gunPos) + np.array([0, 0, 0.165]) rayOrn = np.array([0, 0, 1]) rayDist = 0.25 #rayOrn = np.array([0,0,1]) #Check and see if the ray intersects both objects rayTest = p.rayTest(rayPos, rayPos + rayDist * rayOrn)[0] hitID = rayTest[0] hitPos = np.array(rayTest[3]) #Add constrain constraintID = p.createConstraint(object1ID, -1, hitID, -1, p.JOINT_FIXED, hitPos, parentRefCOM, childRefCOM, childFrameOrientation=childFrameOrn) print("Finished") print(p.getConstraintInfo(constraintID)) return constraintID
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 add_fixed_constraint(body, robot, robot_link, max_force=None): body_link = BASE_LINK body_pose = body.get_pose() #body_pose = get_com_pose(body, link=body_link) #end_effector_pose = get_link_pose(robot, robot_link) end_effector_pose = robot.get_com_pose(robot_link) grasp_pose = pb_robot.geometry.multiply(pb_robot.geometry.invert(end_effector_pose), body_pose) point, quat = grasp_pose # TODO: can I do this when I'm not adjacent? # joint axis in local frame (ignored for JOINT_FIXED) #return p.createConstraint(robot, robot_link, body, body_link, # p.JOINT_FIXED, jointAxis=unit_point(), # parentFramePosition=unit_point(), # childFramePosition=point, # parentFrameOrientation=unit_quat(), # childFrameOrientation=quat) constraint = p.createConstraint(robot, robot_link, body, body_link, # Both seem to work p.JOINT_FIXED, jointAxis=pb_robot.geometry.unit_point(), parentFramePosition=point, childFramePosition=pb_robot.geometry.unit_point(), parentFrameOrientation=quat, childFrameOrientation=pb_robot.geometry.unit_quat(), physicsClientId=CLIENT) if max_force is not None: p.changeConstraint(constraint, maxForce=max_force, physicsClientId=CLIENT) return constraint
def reset(self): objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf")) self.kukaUid = objects[0] p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000]) self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ] self.numJoints = p.getNumJoints(self.kukaUid) for jointIndex in range (self.numJoints): p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex]) #p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,0,0) self.endEffectorPos = [0.537,0.0,0.5] self.endEffectorAngle = 0 self.motorNames = [] self.motorIndices = [] import random xpos = 0.55 +0.12*random.random() ypos = 0 +0.2*random.random() ang = 3.14*0.5+3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) orn = np.array((-0.0, 1, 0.0, 0.0)) pos = (0.5321085918022986, -0.0011177175302174629, 0.2522914797947443), self.constraint = p.createConstraint(self.kukaUid,7,-1,-1,p.JOINT_FIXED,pos,orn,[0.500000,0.300006,0.700000])
def load_agent(self): obj = pb.getBasePositionAndOrientation(self.obj_to_classify) target = obj[0] xyz = [target[0] - 1.25, target[1], target[2]] #xyz=[0,-1.25,0] orientation = [0, 1, 0, 1] self.agent = pb.createCollisionShape( pb.GEOM_BOX, halfExtents=[self.wandSide, self.wandSide, self.wandLength]) self.agent_mb = pb.createMultiBody(1, self.agent, -1, basePosition=xyz, baseOrientation=orientation) pb.changeVisualShape(self.agent_mb, -1, rgbaColor=[1, 1, 0, 1]) self.agent_cid = pb.createConstraint(self.agent_mb, -1, -1, -1, pb.JOINT_FIXED, [0, 0, 0], [0, 0, 0], xyz, childFrameOrientation=orientation) self.fov = 90
def __init__(self, server, **kwargs): super().__init__(server, **kwargs) # Add the constraints parents = ["top_11", "top_12", "top_13"] children = { "top_11": ["ITF_31"], "top_12": ["ITF_22", "ITF_12"], "top_13": ["ITF_23", "ITF_33"], } self.constraints = {} for parent in parents: parent_id = self.joints[parent]["jointIndex"] for child in children[parent]: constraint_name = parent + "_2_" + child child_id = self.joints[child]["jointIndex"] # Create a p2p connection constraint_id = pybullet.createConstraint( parentBodyUniqueId=self.id, parentLinkIndex=parent_id, childBodyUniqueId=self.id, childLinkIndex=child_id, jointType=pybullet.JOINT_POINT2POINT, jointAxis=(0, 0, 0), parentFramePosition=(0, 0, 0), childFramePosition=(0, 0, 0), ) # Store the constraint information self.constraints.update({constraint_name: constraint_id})
def attach(self, object, link=None, loose=False): """ This method attaches an other object to this object. This is done by saving the transformation between the given link, if there is one, and the base pose of the other object. Additional the name of the link, to which the obejct is attached, will be saved. Furthermore, a constraint of pybullet will be created so the attachment also works in the simulation. :param object: The other object that should be attached :param link: The link of this obejct to which the other object should be attached. """ link_id = self.get_link_id(link) if link else -1 link_T_object = self._calculate_transform(object, link) self.attachments[object] = [link_T_object, link, loose] object.attachments[self] = [ p.invertTransform(link_T_object[0], link_T_object[1]), None, False ] cid = p.createConstraint(self.id, link_id, object.id, -1, p.JOINT_FIXED, [0, 1, 0], link_T_object[0], [0, 0, 0], link_T_object[1], physicsClientId=self.world.client_id) self.cids[object] = cid object.cids[self] = cid self.world.attachment_event(self, [self, object])
def robot_specific_reset(self): WalkerBase.robot_specific_reset(self) humanoidId = -1 numBodies = p.getNumBodies() for i in range (numBodies): bodyInfo = p.getBodyInfo(i) if bodyInfo[1].decode("ascii") == 'humanoid': humanoidId = i ## Spherical radiance/glass shield to protect the robot's camera if self.glass_id is None: glass_id = p.loadMJCF(os.path.join(self.physics_model_dir, "glass.xml"))[0] #print("setting up glass", glass_id, humanoidId) p.changeVisualShape(glass_id, -1, rgbaColor=[0, 0, 0, 0]) cid = p.createConstraint(humanoidId, -1, glass_id,-1,p.JOINT_FIXED,[0,0,0],[0,0,1.4],[0,0,1]) self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"] self.motor_power = [100, 100, 100] self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"] self.motor_power += [100, 100, 300, 200] self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"] self.motor_power += [100, 100, 300, 200] self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"] self.motor_power += [75, 75, 75] self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"] self.motor_power += [75, 75, 75] self.motors = [self.jdict[n] for n in self.motor_names]
def reset(self): self.panda_id = p.loadURDF(self.urdf_root_path, basePosition=self.base_position, useFixedBase=True) for i in range(self.motor_count): p.resetJointState(self.panda_id, i, self.start_joint_positions[i]) p.setJointMotorControl2( self.panda_id, i, p.POSITION_CONTROL, targetPosition=self.start_joint_positions[i], force=self.max_force) state = p.getLinkState(self.panda_id, self.gripper_index) for j in range(p.getNumJoints(self.panda_id)): p.changeDynamics(self.panda_id, j, linearDamping=0, angularDamping=0) c = p.createConstraint(self.panda_id, 9, self.panda_id, 10, jointType=p.JOINT_GEAR, jointAxis=[1, 0, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(c, gearRatio=-1, erp=0.1, maxForce=50) self.gripper_pos = list(state[0]) self.gripper_orn = list(p.getEulerFromQuaternion(list(state[1])))
def create_constraint(self, parent_link, child, child_link, joint_type=p.JOINT_FIXED, joint_axis=[0, 0, 0], parent_pos=[0, 0, 0], child_pos=[0, 0, 0], parent_orient=[0, 0, 0], child_orient=[0, 0, 0]): if len(parent_orient) < 4: parent_orient = self.get_quaternion(parent_orient) if len(child_orient) < 4: child_orient = self.get_quaternion(child_orient) return p.createConstraint(self.body, parent_link, child.body, child_link, joint_type, joint_axis, parent_pos, child_pos, parent_orient, child_orient, physicsClientId=self.id)
def __init__(self, robot, ee, obj_ids): # pylint: disable=unused-argument """Creates spatula and 'attaches' it to the robot.""" super().__init__() # Load spatula model. pose = ((0.487, 0.109, 0.438), p.getQuaternionFromEuler((np.pi, 0, 0))) base = p.loadURDF('assets/ur5/spatula/spatula-base.urdf', pose[0], pose[1]) p.createConstraint( parentBodyUniqueId=robot, parentLinkIndex=ee, childBodyUniqueId=base, childLinkIndex=-1, jointType=p.JOINT_FIXED, jointAxis=(0, 0, 0), parentFramePosition=(0, 0, 0), childFramePosition=(0, 0, 0.01))
def activate(self): """Simulate suction using a rigid fixed constraint to contacted object.""" # TODO(andyzeng): check deformables logic. # del def_ids if not self.activated: points = p.getContactPoints(bodyA=self.body, linkIndexA=0) # print(points) if points: # Handle contact between suction with a rigid object. for point in points: obj_id, contact_link = point[2], point[4] if obj_id in self.obj_ids['rigid']: body_pose = p.getLinkState(self.body, 0) obj_pose = p.getBasePositionAndOrientation(obj_id) world_to_body = p.invertTransform(body_pose[0], body_pose[1]) obj_to_body = p.multiplyTransforms(world_to_body[0], world_to_body[1], obj_pose[0], obj_pose[1]) self.contact_constraint = p.createConstraint( parentBodyUniqueId=self.body, parentLinkIndex=0, childBodyUniqueId=obj_id, childLinkIndex=contact_link, jointType=p.JOINT_FIXED, jointAxis=(0, 0, 0), parentFramePosition=obj_to_body[0], parentFrameOrientation=obj_to_body[1], childFramePosition=(0, 0, 0), childFrameOrientation=(0, 0, 0)) self.activated = True
def load_agent(self): xyz = [-1, -1, 0] orientation = [0, 1, 0, 1] self.agent = pb.createCollisionShape( pb.GEOM_BOX, halfExtents=[self.wandSide, self.wandSide, self.wandLength]) self.agent_mb = pb.createMultiBody(1, self.agent, -1, basePosition=xyz, baseOrientation=orientation) pb.changeVisualShape(self.agent, -1, rgbaColor=[1, 1, 0, 1]) self.agent_cid = pb.createConstraint(self.agent, -1, -1, -1, pb.JOINT_FIXED, [0, 0, 0], [0, 0, 0], xyz, childFrameOrientation=orientation) self.fov = 45
def create_constraint_local(self, parentBody, childBody, jointType='fixed', parentLink=None, childLink=None, jointAxis=[1, 0, 0], parentJointPosition=[0, 0, 0], childJointPosition=[0, 0, 0], parentJointOrientation=[0, 0, 0, 1], childJointOrientation=[0, 0, 0, 1], name_override=None): if name_override is None: counter = 0 constraintName = 'constraint_{}'.format(jointType.lower()) constraintId = constraintName while constraintId in self.bodies: constraintId = '{}.{}'.format(bodyName, counter) counter += 1 else: if name_override in self.constraints: raise Exception('Constraint Id "{}" is already taken.'.format( name_override)) constraintId = name_override parent_bid = parentBody.bId() parent_link = parentBody.get_link_index( parentLink) if parentLink is not None else -1 child_bid = childBody.bId() if childBody is not None else -1 child_link = childBody.get_link_index( childLink) if childLink is not None else -1 if jointType not in self.__joint_types: raise Exception( 'Unknown joint type "{}". Supported types are: {}'.format( jointType, ', '.join(self.__joint_types.keys()))) type = self.__joint_types[jointType] bulletId = pb.createConstraint(parent_bid, parent_link, child_bid, child_link, type, axis, parentJointPosition, childJointPosition, parentJointOrientation, childJointOrientation, physicsClientId=self.__client_id) constraint = Constraint(self, bulletId, jointType, parentBody, childBody, parentJointPosition, parentJointOrientation, childJointPosition, childJointOrientation, jointAxis, parentLink, childLink) self.constraints[constraintId] = constraint self.__cId_IdMap[bulletId] = constraintId return constraint
def _setup_experiment(self): # add plane to push on (slightly below the base of the robot) self.planeId = p.loadURDF("plane.urdf", [0, 0, 0], useFixedBase=True) self.pusherId = p.loadURDF(os.path.join(cfg.ROOT_DIR, "pusher.urdf"), self.initPusherPos) self.blockId = p.loadURDF( os.path.join(cfg.ROOT_DIR, "block_big.urdf"), self.initBlockPos, p.getQuaternionFromEuler([0, 0, self.initBlockYaw])) # adjust dynamics for better stability p.changeDynamics(self.planeId, -1, lateralFriction=0.3, spinningFriction=0.0, rollingFriction=0.0) self.walls = [] wall_z = 0.05 if self.level == 0: pass elif self.level in [1, 4, 5]: self.walls.append( p.loadURDF(os.path.join(cfg.ROOT_DIR, "wall.urdf"), [-0.55, -0.25, wall_z], p.getQuaternionFromEuler([0, 0, 0]), useFixedBase=True, globalScaling=0.8)) elif self.level == 2: translation = 10 self.walls.append( p.loadURDF(os.path.join(cfg.ROOT_DIR, "wall.urdf"), [-0.55 + translation, -0.25 + translation, wall_z], p.getQuaternionFromEuler([0, 0, 0]), useFixedBase=True, globalScaling=0.8)) elif self.level in [3, 6]: self.walls.append( p.loadURDF(os.path.join(cfg.ROOT_DIR, "wall.urdf"), [-0.3, 0.25, wall_z], p.getQuaternionFromEuler([0, 0, -math.pi / 4]), useFixedBase=True, globalScaling=0.8)) if self.level == 2: self.set_camera_position([10, 10]) else: self.set_camera_position([0, 0]) self._draw_goal() self.state = self._obs() self._draw_state() # set gravity p.setGravity(0, 0, -10) # set robot init config self.pusherConstraint = p.createConstraint(self.pusherId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0], self.initPusherPos)
def resetPose(self): kneeFrictionForce = 0 halfpi = 1.57079632679 kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length) #left front leg p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle) p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi) self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) #left back leg p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle) p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi) self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) #right front leg p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle) p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi) self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) #right back leg p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle) p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi) self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
def resetPose(self): #right front leg self.disableAllMotors() p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2) p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) self.setMotorAngleByName('motor_front_rightR_joint', 1.57) self.setMotorAngleByName('motor_front_rightL_joint',-1.57) #left front leg p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2) p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) self.setMotorAngleByName('motor_front_leftR_joint', 1.57) self.setMotorAngleByName('motor_front_leftL_joint',-1.57) #right back leg p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2) p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) self.setMotorAngleByName('motor_back_rightR_joint', 1.57) self.setMotorAngleByName('motor_back_rightL_joint',-1.57) #left back leg p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2) p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) self.setMotorAngleByName('motor_back_leftR_joint', 1.57) self.setMotorAngleByName('motor_back_leftL_joint',-1.57)
import pybullet as p import time import math p.connect(p.SHARED_MEMORY) p.loadURDF("plane.urdf") p.setGravity(0, 0, -1) p.setRealTimeSimulation(0) quadruped = p.loadURDF("quadruped/quadruped.urdf", 10, -2, 2) # p.getNumJoints(1) # right front leg p.resetJointState(quadruped, 0, 1.57) p.resetJointState(quadruped, 2, -2.2) p.resetJointState(quadruped, 3, -1.57) p.resetJointState(quadruped, 5, 2.2) p.createConstraint(quadruped, 2, quadruped, 5, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.01, 0.2], [0, -0.015, 0.2]) p.setJointMotorControl(quadruped, 0, p.POSITION_CONTROL, 1.57, 1) p.setJointMotorControl(quadruped, 1, p.VELOCITY_CONTROL, 0, 0) p.setJointMotorControl(quadruped, 2, p.VELOCITY_CONTROL, 0, 0) p.setJointMotorControl(quadruped, 3, p.POSITION_CONTROL, -1.57, 1) p.setJointMotorControl(quadruped, 4, p.VELOCITY_CONTROL, 0, 0) p.setJointMotorControl(quadruped, 5, p.VELOCITY_CONTROL, 0, 0) # left front leg p.resetJointState(quadruped, 6, 1.57) p.resetJointState(quadruped, 8, -2.2) p.resetJointState(quadruped, 9, -1.57) p.resetJointState(quadruped, 11, 2.2) p.createConstraint(quadruped, 8, quadruped, 11, p.JOINT_POINT2POINT, [0, 0, 0], [0, -0.01, 0.2], [0, 0.015, 0.2])
import pybullet as p import time import math p.connect(p.SHARED_MEMORY) p.loadURDF("plane.urdf") quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3) #p.getNumJoints(1) #right front leg p.resetJointState(quadruped,0,1.57) p.resetJointState(quadruped,2,-2.2) p.resetJointState(quadruped,3,-1.57) p.resetJointState(quadruped,5,2.2) p.createConstraint(quadruped,2,quadruped,5,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,1.57,1) p.setJointMotorControl(quadruped,1,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,2,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1) p.setJointMotorControl(quadruped,4,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,5,p.VELOCITY_CONTROL,0,0) #left front leg p.resetJointState(quadruped,6,1.57) p.resetJointState(quadruped,8,-2.2) p.resetJointState(quadruped,9,-1.57) p.resetJointState(quadruped,11,2.2) p.createConstraint(quadruped,8,quadruped,11,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,1.57,1)
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)] objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)] objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)] #load the MuJoCo MJCF hand objects = p.loadMJCF("MPL/mpl2.xml") hand=objects[0] ho = p.getQuaternionFromEuler([3.14,-3.14/2,0]) hand_cid = p.createConstraint(hand,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[-0.05,0,0.02],[0.500000,0.300006,0.700000],ho) print ("hand_cid") print (hand_cid) for i in range (p.getNumJoints(hand)): p.setJointMotorControl2(hand,i,p.POSITION_CONTROL,0,0) #clamp in range 400-600 #minV = 400 #maxV = 600 minV = 250 maxV = 450 POSITION=1 ORIENTATION=2
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(useRealTimeSim) # either this p.loadURDF("plane.urdf") #p.loadSDF("stadium.sdf") car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True) for i in range (p.getNumJoints(car)): print (p.getJointInfo(car,i)) for wheel in range(p.getNumJoints(car)): p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0) p.getJointInfo(car,wheel) wheels = [8,15] print("----------------") #p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10) c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=1, maxForce=10000) c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=-1, maxForce=10000) c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=-1, maxForce=10000) c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=1, maxForce=10000) c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=-1, maxForce=10000)
p.loadURDF("quadruped/minitaur.urdf", [-0.000046, -0.000068, 0.200774], [-0.000701, 0.000387, -0.000252, 1.000000], useFixedBase=False) ] ob = objects[0] jointPositions = [ 0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000, -2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193, 0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517 ] for ji in range(p.getNumJoints(ob)): p.resetJointState(ob, ji, jointPositions[ji]) p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0) cid0 = p.createConstraint(1, 3, 1, 6, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000], [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000], [0.000000, 0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 0.000000, 1.000000]) p.changeConstraint(cid0, maxForce=500.000000) cid1 = p.createConstraint(1, 16, 1, 19, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000], [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000], [0.000000, 0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 0.000000, 1.000000]) p.changeConstraint(cid1, maxForce=500.000000) cid2 = p.createConstraint(1, 9, 1, 12, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000], [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000], [0.000000, 0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 0.000000, 1.000000]) p.changeConstraint(cid2, maxForce=500.000000) cid3 = p.createConstraint(1, 22, 1, 25, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000], [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000], [0.000000, 0.000000, 0.000000, 1.000000],
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])
p.resetSimulation() #disable rendering during loading makes it much faster p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] #objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)] pr2_gripper = objects[0] print ("pr2_gripper=") print (pr2_gripper) jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ] for jointIndex in range (p.getNumJoints(pr2_gripper)): p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex]) p.setJointMotorControl2(pr2_gripper,jointIndex,p.POSITION_CONTROL,targetPosition=0,force=0) pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000]) print ("pr2_cid") print (pr2_cid) pr2_cid2 = p.createConstraint(pr2_gripper,0,pr2_gripper,2,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(pr2_cid2,gearRatio=1, erp=0.5, relativePositionTarget=0.5, maxForce=3) objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)] kuka = objects[0] jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ] for jointIndex in range (p.getNumJoints(kuka)): p.resetJointState(kuka,jointIndex,jointPositions[jointIndex]) p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
p.connect(p.GUI) p.resetSimulation() #disable rendering during loading makes it much faster p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)] pr2_gripper = objects[0] print ("pr2_gripper=") print (pr2_gripper) jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ] for jointIndex in range (p.getNumJoints(pr2_gripper)): p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex]) pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000]) print ("pr2_cid") print (pr2_cid) objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)] kuka = objects[0] jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ] for jointIndex in range (p.getNumJoints(kuka)): p.resetJointState(kuka,jointIndex,jointPositions[jointIndex]) p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)] objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf") kuka_gripper = objects[0]
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC", "JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"] startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]] p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0]) p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0]) p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0]) p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0]) humanoid = p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) humanoid2 = p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) humanoid3 = p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) humanoid4 = p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1]) humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1]) humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1]) humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1]) startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447, 1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153, 0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495, 0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348] startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-0.1050802848,0,0.935706195,0.08277326387,0.3002461862,0,0,0,0,0,1.114409628,0.3618553952, -0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0, -4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973] p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1])
motC_lb_Id= p.addUserDebugParameter("motC_lb",-limitVal,limitVal,legposSleft) erp_lb_Id= p.addUserDebugParameter("erp_lb",0,1,defaultERP) relPosTarget_lb_Id= p.addUserDebugParameter("relPosTarget_lb",-limitVal,limitVal,0) camTargetPos=[0.25,0.62,-0.15] camDist = 2 camYaw = -2 camPitch=-16 p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos) c_rf = p.createConstraint(vision,knee_front_rightR_joint,vision,motor_front_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,maxForce=maxGearForce) c_lf = p.createConstraint(vision,knee_front_leftL_joint,vision,motor_front_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,maxForce=maxGearForce) c_rb = p.createConstraint(vision,knee_back_rightR_joint,vision,motor_back_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,maxForce=maxGearForce) c_lb = p.createConstraint(vision,knee_back_leftL_joint,vision,motor_back_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,maxForce=maxGearForce) p.setRealTimeSimulation(1)