def joinUrdf(self, childEditor, parentLinkIndex=0, jointPivotXYZInParent=[0,0,0], jointPivotRPYInParent=[0,0,0], jointPivotXYZInChild=[0,0,0], jointPivotRPYInChild=[0,0,0], parentPhysicsClientId=0, childPhysicsClientId=0): childLinkIndex = len(self.urdfLinks) insertJointIndex = len(self.urdfJoints) #combine all links, and add a joint for link in childEditor.urdfLinks: self.linkNameToIndex[link.link_name]=len(self.urdfLinks) self.urdfLinks.append(link) for joint in childEditor.urdfJoints: self.urdfJoints.append(joint) #add a new joint between a particular jointPivotQuatInChild = p.getQuaternionFromEuler(jointPivotRPYInChild) invJointPivotXYZInChild, invJointPivotQuatInChild = p.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild) #apply this invJointPivot***InChild to all inertial, visual and collision element in the child link #inertial pos, orn = p.multiplyTransforms(self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,p.getQuaternionFromEuler(self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild, physicsClientId=parentPhysicsClientId) self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p.getEulerFromQuaternion(orn) #all visual for v in self.urdfLinks[childLinkIndex].urdf_visual_shapes: pos, orn = p.multiplyTransforms(v.origin_xyz,p.getQuaternionFromEuler(v.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild) v.origin_xyz = pos v.origin_rpy = p.getEulerFromQuaternion(orn) #all collision for c in self.urdfLinks[childLinkIndex].urdf_collision_shapes: pos, orn = p.multiplyTransforms(c.origin_xyz,p.getQuaternionFromEuler(c.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild) c.origin_xyz = pos c.origin_rpy = p.getEulerFromQuaternion(orn) childLink = self.urdfLinks[childLinkIndex] parentLink = self.urdfLinks[parentLinkIndex] joint = UrdfJoint() joint.link = childLink joint.joint_name = "joint_dummy1" joint.joint_type = p.JOINT_REVOLUTE joint.joint_lower_limit = 0 joint.joint_upper_limit = -1 joint.parent_name = parentLink.link_name joint.child_name = childLink.link_name joint.joint_origin_xyz = jointPivotXYZInParent joint.joint_origin_rpy = jointPivotRPYInParent joint.joint_axis_xyz = [0,0,1] #the following commented line would crash PyBullet, it messes up the joint indexing/ordering #self.urdfJoints.append(joint) #so make sure to insert the joint in the right place, to links/joints match self.urdfJoints.insert(insertJointIndex,joint) return joint
def _randomly_place_objects(self, urdfList): """Randomly places the objects in the bin. Args: urdfList: The list of urdf files to place in the bin. Returns: The list of object unique ID's. """ # Randomize positions of each object urdf. objectUids = [] for urdf_name in urdfList: xpos = 0.4 +self._blockRandom*random.random() ypos = self._blockRandom*(random.random()-.5) angle = np.pi/2 + self._blockRandom * np.pi * random.random() orn = p.getQuaternionFromEuler([0, 0, angle]) urdf_path = os.path.join(self._urdfRoot, urdf_name) uid = p.loadURDF(urdf_path, [xpos, ypos, .15], [orn[0], orn[1], orn[2], orn[3]]) objectUids.append(uid) # Let each object fall to the tray individual, to prevent object # intersection. for _ in range(500): p.stepSimulation() return objectUids
def _reset(self): self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) xpos = 0.5 +0.2*random.random() ypos = 0 +0.25*random.random() ang = 3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation)
#coord = p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True) p.setGravity(0,0,-10) p.setRealTimeSimulation(0) #coordPos = [0,0,0] #coordOrnEuler = [0,0,0] #coordPos= [0.7000000000000004, 0, 0.22000000000000006] #coordOrnEuler= [0, -0.2617993877991496, 0] coordPos= [0.07, 0, -0.6900000000000004] coordOrnEuler= [0, 0, 0] coordPos2= [0, 0, 0 ] coordOrnEuler2= [0, 0, 0] invPos,invOrn=p.invertTransform(coordPos,p.getQuaternionFromEuler(coordOrnEuler)) mPos,mOrn = p.multiplyTransforms(invPos,invOrn, coordPos2,p.getQuaternionFromEuler(coordOrnEuler2)) eul = p.getEulerFromQuaternion(mOrn) print("rpy=\"",eul[0],eul[1],eul[2],"\" xyz=\"", mPos[0],mPos[1], mPos[2]) shift=0 gui = 1 frame=0 states=[] states.append(p.saveState()) #observations=[] #observations.append(obs)
def reset(self): self.setup_timing() self.task_success = 0 self.forearm_in_sleeve = False self.upperarm_in_sleeve = False self.meanft = np.zeros(6) self.wheelchair = self.world_creation.create_new_world(furniture_type='wheelchair', static_human_base=True, human_impairment='random', gender='random') if self.robot.wheelchair_mounted: wheelchair_pos, wheelchair_orient = self.wheelchair.get_base_pos_orient() self.robot.set_base_pos_orient(wheelchair_pos + np.array(self.robot.toc_base_pos_offset[self.task]), [0, 0, -np.pi/2.0], euler=True) joints_positions = [(self.human.j_right_elbow, -90), (self.human.j_left_shoulder_x, -80), (self.human.j_left_elbow, -90), (self.human.j_right_hip_x, -90), (self.human.j_right_knee, 80), (self.human.j_left_hip_x, -90), (self.human.j_left_knee, 80)] self.human.setup_joints(joints_positions, use_static_joints=True, reactive_force=None if self.human_control else 1, reactive_gain=0.01) shoulder_pos = self.human.get_pos_orient(self.human.left_shoulder)[0] elbow_pos = self.human.get_pos_orient(self.human.left_elbow)[0] wrist_pos = self.human.get_pos_orient(self.human.left_wrist)[0] target_ee_pos = np.array([0.45, -0.3, 1.2]) + self.np_random.uniform(-0.05, 0.05, size=3) target_ee_orient = np.array(p.getQuaternionFromEuler(np.array(self.robot.toc_ee_orient_rpy[self.task][0]), physicsClientId=self.id)) offset = np.array([0, 0, 0.1]) if self.robot.wheelchair_mounted: # Use IK to find starting joint angles for mounted robots self.robot.ik_random_restarts(right=False, target_pos=target_ee_pos, target_orient=target_ee_orient, max_iterations=1000, max_ik_random_restarts=40, success_threshold=0.03, step_sim=True, check_env_collisions=False) else: # Use TOC with JLWKI to find an optimal base position for the robot near the person target_ee_orient_shoulder = np.array(p.getQuaternionFromEuler(np.array(self.robot.toc_ee_orient_rpy[self.task][1]), physicsClientId=self.id)) self.robot.position_robot_toc(self.task, 'left', [(target_ee_pos, target_ee_orient)], [(shoulder_pos+offset, target_ee_orient_shoulder), (elbow_pos+offset, target_ee_orient), (wrist_pos+offset, target_ee_orient)], base_euler_orient=[0, 0, np.pi], step_sim=True, check_env_collisions=False, right_side=False) # Open gripper to hold the tool self.robot.set_gripper_open_position(self.robot.left_gripper_indices, self.robot.gripper_pos[self.task], set_instantly=True) if self.human_control or self.human.impairment == 'tremor': # Ensure the human arm remains stable while loading the cloth self.human.control(self.human.controllable_joint_indices, self.human.get_joint_angles(self.human.controllable_joint_indices), 0.05, 1) self.start_ee_pos, self.start_ee_orient = self.robot.get_pos_orient(self.robot.left_end_effector) self.cloth_orig_pos = np.array([0.34658437, -0.30296362, 1.20023387]) self.cloth_offset = self.start_ee_pos - self.cloth_orig_pos # Setup gripper - cloth constraint # NOTE: Mass of cloth_attachment determines the stiffness of the anchors! sphere_collision_1, sphere_visual_1 = self.world_creation.create_sphere(radius=0.0001, mass=0.1, pos=[0, 0, 0], visual=True, collision=False, rgba=[0, 0, 0, 0], return_collision_visual=True) sphere_collision_2, sphere_visual_2 = self.world_creation.create_sphere(radius=0.0001, mass=0.1, pos=[0, 0, 0], visual=True, collision=False, rgba=[0, 0, 0, 0], return_collision_visual=True) end_effector_attachment = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=sphere_collision_1, baseVisualShapeIndex=sphere_visual_1, basePosition=self.start_ee_pos, baseOrientation=[0, 0, 0, 1], linkMasses=[0.1], linkCollisionShapeIndices=[sphere_collision_2], linkVisualShapeIndices=[sphere_visual_2], linkPositions=[[0, 0, 0]], linkOrientations=[[0, 0, 0, 1]], linkInertialFramePositions=[[0, 0, 0]], linkInertialFrameOrientations=[[0, 0, 0, 1]], linkParentIndices=[0], linkJointTypes=[p.JOINT_REVOLUTE], linkJointAxis=[[1, 1, 1]], linkLowerLimits=[0], linkUpperLimits=[0], useMaximalCoordinates=False, physicsClientId=self.id) # end_effector_attachment = p.loadURDF(os.path.join(self.world_creation.directory, 'clothing', 'ft_sensor.urdf'), physicsClientId=self.id) self.end_effector_attachment = Agent() self.end_effector_attachment.init(end_effector_attachment, self.id, self.np_random, indices=-1) self.cloth_attachment = self.world_creation.create_sphere(radius=0.0001, mass=1, pos=self.start_ee_pos, visual=True, collision=False, rgba=[0, 0, 0, 0], maximal_coordinates=True) # self.robot.create_constraint(self.robot.left_end_effector, self.end_effector_attachment, self.end_effector_attachment.base, parent_orient=[0, 0, -np.pi/2.0]) self.end_effector_attachment.create_constraint(0, self.cloth_attachment, self.cloth_attachment.base) self.end_effector_attachment.enable_force_torque_sensor(0) # self.cloth_attachment = self.world_creation.create_sphere(radius=0.0001, mass=1, pos=self.start_ee_pos, visual=True, collision=True, rgba=[0, 0, 0, 0], maximal_coordinates=True) # # self.robot.create_constraint(self.robot.left_end_effector, self.cloth_attachment, self.cloth_attachment.base, parent_pos=[0.1, 0, 0], parent_orient=[0, 0, -np.pi/2.0]) # self.robot.create_constraint(self.robot.left_end_effector, self.cloth_attachment, self.cloth_attachment.base) # self.robot.enable_force_torque_sensor(self.robot.left_end_effector) # Load cloth # self.cloth = p.loadCloth(os.path.join(self.world_creation.directory, 'clothing', 'hospitalgown_reduced.obj'), scale=1.2, mass=0.16, position=np.array([0.4, -0.42, 0.99]) + self.cloth_offset/1.2, orientation=p.getQuaternionFromEuler([0, 0, np.pi], physicsClientId=self.id), bodyAnchorId=self.cloth_attachment.body, anchors=[3831, 3558, 1877, 3708, 1974, 2707, 3201, 637, 2405, 3279, 2708, 2592, 2407, 445, 509, 577], collisionMargin=0.04, rgbaColor=np.array([139./256., 195./256., 74./256., 0.6]), rgbaLineColor=np.array([197./256., 225./256., 165./256., 1]), physicsClientId=self.id) # p.clothParams(self.cloth, kLST=0.055, kAST=1.0, kVST=0.5, kDP=0.05, kDG=10, kDF=0.39, kCHR=2.38, kKHR=2.0, kAHR=2.0, piterations=2, physicsClientId=self.id) # # Points along the opening of sleeve # self.triangle1_point_indices = [1814, 1869, 1121] # self.triangle2_point_indices = [2115, 540, 365] self.cloth = p.loadCloth(os.path.join(self.world_creation.directory, 'clothing', 'hospitalgown_reduced.obj'), scale=1.4, mass=0.16, position=np.array([0.02, -0.38, 0.83]) + self.cloth_offset/1.4, orientation=p.getQuaternionFromEuler([0, 0, np.pi], physicsClientId=self.id), bodyAnchorId=self.cloth_attachment.body, anchors=[2087, 3879, 3681, 3682, 2086, 2041, 987, 2042, 2088, 1647, 2332, 719, 1569, 1528, 721], collisionMargin=0.04, rgbaColor=np.array([139./256., 195./256., 74./256., 0.6]), rgbaLineColor=np.array([197./256., 225./256., 165./256., 1]), physicsClientId=self.id) # p.clothParams(self.cloth, kLST=0.055, kAST=1.0, kVST=0.5, kDP=0.005, kDG=10, kDF=0.39, kCHR=1.0, kKHR=1.0, kAHR=1.0, piterations=5, physicsClientId=self.id) p.clothParams(self.cloth, kLST=0.055, kAST=1.0, kVST=0.5, kDP=0.01, kDG=10, kDF=0.39, kCHR=1.0, kKHR=1.0, kAHR=1.0, piterations=5, physicsClientId=self.id) # Points along the opening of sleeve self.triangle1_point_indices = [1180, 2819, 30] self.triangle2_point_indices = [1322, 13, 696] # Points along the end of sleeve # self.triangle1_point_indices = [621, 37, 1008] # self.triangle2_point_indices = [130, 3908, 2358] # double m_kLST; // Material: Linear stiffness coefficient [0,1] # double m_kAST; // Material: Area/Angular stiffness coefficient [0,1] # double m_kVST; // Material: Volume stiffness coefficient [0,1] # double m_kVCF; // Velocities correction factor (Baumgarte) # double m_kDP; // Damping coefficient [0,1] # double m_kDG; // Drag coefficient [0,+inf] # double m_kLF; // Lift coefficient [0,+inf] # double m_kPR; // Pressure coefficient [-inf,+inf] # double m_kVC; // Volume conversation coefficient [0,+inf] # double m_kDF; // Dynamic friction coefficient [0,1] # double m_kMT; // Pose matching coefficient [0,1] # double m_kCHR; // Rigid contacts hardness [0,1] # double m_kKHR; // Kinetic contacts hardness [0,1] # double m_kSHR; // Soft contacts hardness [0,1] # double m_kAHR; // Anchors hardness [0,1] # int m_viterations; // Velocities solver iterations # int m_piterations; // Positions solver iterations # int m_diterations; // Drift solver iterations p.setGravity(0, 0, -9.81/2, physicsClientId=self.id) # Let the cloth settle more gently p.setGravity(0, 0, 0, body=self.robot.body, physicsClientId=self.id) p.setGravity(0, 0, -1, body=self.human.body, physicsClientId=self.id) p.setGravity(0, 0, 0, body=self.cloth_attachment.body, physicsClientId=self.id) p.setGravity(0, 0, 0, body=self.end_effector_attachment.body, physicsClientId=self.id) p.setPhysicsEngineParameter(numSubSteps=8, physicsClientId=self.id) # Enable rendering p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id) # Wait for the cloth to settle for _ in range(50): # Force the end effector attachment to stay at the end effector self.end_effector_attachment.set_base_pos_orient(self.start_ee_pos, [0, 0, 0, 1]) # self.cloth_attachment.set_base_pos_orient(self.start_ee_pos, [0, 0, 0, 1]) p.stepSimulation(physicsClientId=self.id) p.setGravity(0, 0, -9.81, physicsClientId=self.id) return self._get_obs([0]*6, [0, 0])
global_axis = gl.GLAxisItem() global_axis.updateGLOptions({'glLineWidth': (4, )}) window.addItem(global_axis) print("WINDOW not updated") window.update() # configure pybullet and load plane.urdf and quadcopter.urdf physicsClient = p.connect( p.DIRECT) # pybullet only for computations no visualisation p.setGravity(0, 0, -gravity) p.setTimeStep(Tsample_physics) # disable real-time simulation, we want to step through the # physics ourselves with p.stepSimulation() p.setRealTimeSimulation(0) planeId = p.loadURDF("plane.urdf", [0, 0, 0], p.getQuaternionFromEuler([0, 0, 0])) quadcopterId = p.loadURDF("quadrotor.urdf", [0, 0, 1], p.getQuaternionFromEuler([0, 0, 0])) # do a few steps to start simulation and let the quadcopter land safely for i in range(int(2 / Tsample_physics)): p.stepSimulation() # create a pyqtgraph mesh from the quadcopter to visualize # the quadcopter in the 3D pyqtgraph window quadcopterMesh = bullet2pyqtgraph(quadcopterId)[0] window.addItem(quadcopterMesh) window.update() # Initialize PID controller gains: Kp = np.zeros(6)
def grasp_primitive(environment, arm, action, xyz, ori, grip, cube_pos, grasped, lifted, img): ## move above loc = np.where(img > 0) # find red pixels if len(loc[0]) == 0: # if there are no red pixels in view action[0] = 0.0 action[1] = 0.0 action[2] = 0.4 else: # seen red pixels, go to them y_relative_dir = np.mean(loc[0]) - cam_dims / 2 x_relative_dir = np.mean(loc[1]) - cam_dims / 2 action[0] = xyz[0] + x_relative_dir * 0.0005 #cube_pos[0] action[1] = xyz[1] - y_relative_dir * 0.0005 y_pixels = loc[1] x_pixels = loc[0] line = np.poly1d(np.polyfit(x_pixels, y_pixels, 1)) # fit a line to determine block angle gradient = line[1] # angle of the block from gripper print(xyz[2], cube_pos[2]) if ((xyz[0] - cube_pos[0]) < 0.01) and ( (xyz[1] - cube_pos[1]) < 0.01) and ( (xyz[2] - cube_pos[2]) < 0.13) and (abs(gradient) < 0.015): if xyz[2] > cube_pos[2] + 0.05: action[2] = xyz[2] - 0.003 else: action[2] = xyz[2] + 0.003 action[7] = grip * 25 + 0.1 # grip is in 0-0.04 scale print('grasping') grasped += 0.5 action[3:7] = p.getQuaternionFromEuler( [ori[0] - gradient * 0.5, ori[1], ori[2]]) # adjust gripper angle to suit block elif ((xyz[0] - cube_pos[0]) < 0.01) and ( (xyz[1] - cube_pos[1]) < 0.01): #we are close enough to move down print('moving to grasp', grasped, lifted) if xyz[2] > cube_pos[2] + 0.2: action[2] = xyz[2] - (xyz[2] - 0.005)**2 else: action[2] = xyz[2] - 0.003 # if xyz[2] > cube_pos[2]+0.05: # action[2] = xyz[2] - 0.003 # else: # action[2] = xyz[2] + 0.003 action[3:7] = p.getQuaternionFromEuler( [ori[0] - gradient * 0.5, ori[1], ori[2]]) # adjust gripper angle to suit block else: # go above it print('moving above') action[2] = cube_pos[2] + 0.2 # adjust gripper angle to suit block return action, grasped, lifted
def createMultiBody(self, basePosition=[0,0,0],baseOrientation=[0,0,0,1], physicsClientId=0): #assume link[0] is base if (len(self.urdfLinks)==0): return -1 #for i in range (len(self.urdfLinks)): # print("link", i, "=",self.urdfLinks[i].link_name) base = self.urdfLinks[0] #v.tmp_collision_shape_ids=[] baseMass = base.urdf_inertial.mass baseCollisionShapeIndex = -1 baseShapeTypeArray=[] baseRadiusArray=[] baseHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] meshScaleArray=[] basePositionsArray=[] baseOrientationsArray=[] for v in base.urdf_collision_shapes: shapeType = v.geom_type baseShapeTypeArray.append(shapeType) baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) baseRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) meshScaleArray.append(v.geom_meshscale) basePositionsArray.append(v.origin_xyz) orn=p.getQuaternionFromEuler(v.origin_rpy) baseOrientationsArray.append(orn) if (len(baseShapeTypeArray)): #print("fileNameArray=",fileNameArray) baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray, radii=baseRadiusArray, halfExtents=baseHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=meshScaleArray, collisionFramePositions=basePositionsArray, collisionFrameOrientations=baseOrientationsArray, physicsClientId=physicsClientId) urdfVisuals = base.urdf_visual_shapes shapeTypes=[v.geom_type for v in urdfVisuals] halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals] radii=[v.geom_radius for v in urdfVisuals] lengths=[v.geom_length for v in urdfVisuals] fileNames=[v.geom_meshfilename for v in urdfVisuals] meshScales=[v.geom_meshscale for v in urdfVisuals] rgbaColors=[v.material_rgba for v in urdfVisuals] visualFramePositions=[v.origin_xyz for v in urdfVisuals] visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals] baseVisualShapeIndex = -1 if (len(shapeTypes)): #print("len(shapeTypes)=",len(shapeTypes)) #print("len(halfExtents)=",len(halfExtents)) #print("len(radii)=",len(radii)) #print("len(lengths)=",len(lengths)) #print("len(fileNames)=",len(fileNames)) #print("len(meshScales)=",len(meshScales)) #print("len(rgbaColors)=",len(rgbaColors)) #print("len(visualFramePositions)=",len(visualFramePositions)) #print("len(visualFrameOrientations)=",len(visualFrameOrientations)) baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes, halfExtents=halfExtents,radii=radii,lengths=lengths,fileNames=fileNames, meshScales=meshScales,rgbaColors=rgbaColors,visualFramePositions=visualFramePositions, visualFrameOrientations=visualFrameOrientations,physicsClientId=physicsClientId) linkMasses=[] linkCollisionShapeIndices=[] linkVisualShapeIndices=[] linkPositions=[] linkOrientations=[] linkInertialFramePositions=[] linkInertialFrameOrientations=[] linkParentIndices=[] linkJointTypes=[] linkJointAxis=[] for joint in self.urdfJoints: link = joint.link linkMass = link.urdf_inertial.mass linkCollisionShapeIndex=-1 linkVisualShapeIndex=-1 linkPosition=[0,0,0] linkOrientation=[0,0,0] linkInertialFramePosition=[0,0,0] linkInertialFrameOrientation=[0,0,0] linkParentIndex=self.linkNameToIndex[joint.parent_name] linkJointType=joint.joint_type linkJointAx = joint.joint_axis_xyz linkShapeTypeArray=[] linkRadiusArray=[] linkHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] linkMeshScaleArray=[] linkPositionsArray=[] linkOrientationsArray=[] for v in link.urdf_collision_shapes: shapeType = v.geom_type linkShapeTypeArray.append(shapeType) linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) linkRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) linkMeshScaleArray.append(v.geom_meshscale) linkPositionsArray.append(v.origin_xyz) linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy)) if (len(linkShapeTypeArray)): linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray, radii=linkRadiusArray, halfExtents=linkHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=linkMeshScaleArray, collisionFramePositions=linkPositionsArray, collisionFrameOrientations=linkOrientationsArray, physicsClientId=physicsClientId) urdfVisuals = link.urdf_visual_shapes linkVisualShapeIndex = -1 shapeTypes=[v.geom_type for v in urdfVisuals] halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals] radii=[v.geom_radius for v in urdfVisuals] lengths=[v.geom_length for v in urdfVisuals] fileNames=[v.geom_meshfilename for v in urdfVisuals] meshScales=[v.geom_meshscale for v in urdfVisuals] rgbaColors=[v.material_rgba for v in urdfVisuals] visualFramePositions=[v.origin_xyz for v in urdfVisuals] visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals] if (len(shapeTypes)): print("fileNames=",fileNames) linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes, halfExtents=halfExtents,radii=radii,lengths=lengths, fileNames=fileNames,meshScales=meshScales,rgbaColors=rgbaColors, visualFramePositions=visualFramePositions, visualFrameOrientations=visualFrameOrientations, physicsClientId=physicsClientId) linkMasses.append(linkMass) linkCollisionShapeIndices.append(linkCollisionShapeIndex) linkVisualShapeIndices.append(linkVisualShapeIndex) linkPositions.append(joint.joint_origin_xyz) linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy)) linkInertialFramePositions.append(link.urdf_inertial.origin_xyz) linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy)) linkParentIndices.append(linkParentIndex) linkJointTypes.append(joint.joint_type) linkJointAxis.append(joint.joint_axis_xyz) obUid = p.createMultiBody(baseMass,\ baseCollisionShapeIndex=baseCollisionShapeIndex, baseVisualShapeIndex=baseVisualShapeIndex, basePosition=basePosition, baseOrientation=baseOrientation, baseInertialFramePosition=base.urdf_inertial.origin_xyz, baseInertialFrameOrientation=p.getQuaternionFromEuler(base.urdf_inertial.origin_rpy), linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=linkParentIndices, linkJointTypes=linkJointTypes, linkJointAxis=linkJointAxis, physicsClientId=physicsClientId) return obUid
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)
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659]) for i in range (p.getNumJoints(husky)): print(p.getJointInfo(husky,i)) kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659) ob = kukaId jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ] for jointIndex in range (p.getNumJoints(ob)): p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) #put kuka on top of husky cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1]) baseorn = p.getQuaternionFromEuler([3.1415,0,0.3]) baseorn = [0,0,0,1] #[0, 0, 0.707, 0.707] #p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1]) kukaEndEffectorIndex = 6 numJoints = p.getNumJoints(kukaId) if (numJoints!=7): exit() #lower limits for null space ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05] #upper limits for null space ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05] #joint ranges for null space
def __init__( self, pos=None, orn=None, useFixedBase=False, init_sliders_pose=4 * [ 0, ], ): # 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(Solo8Config.meshes_path) self.urdf_path = Solo8Config.urdf_path self.robotId = pybullet.loadURDF( self.urdf_path, pos, orn, flags=pybullet.URDF_USE_INERTIA_FROM_FILE, useFixedBase=useFixedBase, ) pybullet.getBasePositionAndOrientation(self.robotId) # Create the robot wrapper in pinocchio. self.pin_robot = Solo8Config.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.slider_a = pybullet.addUserDebugParameter("a", 0, 1, init_sliders_pose[0]) self.slider_b = pybullet.addUserDebugParameter("b", 0, 1, init_sliders_pose[1]) self.slider_c = pybullet.addUserDebugParameter("c", 0, 1, init_sliders_pose[2]) self.slider_d = pybullet.addUserDebugParameter("d", 0, 1, init_sliders_pose[3]) self.base_link_name = "base_link" controlled_joints = [] for leg in ["FL", "FR", "HL", "HR"]: controlled_joints += [leg + "_HFE", leg + "_KFE"] self.joint_names = controlled_joints # Creates the wrapper by calling the super.__init__. super(Solo8Robot, self).__init__( self.robotId, self.pin_robot, controlled_joints, ["FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"], )
def reset(self, ret_images=False, ret_dict=False): self.setup_timing() self.task_success = 0 self.human, self.wheelchair, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world( furniture_type='wheelchair', static_human_base=True, human_impairment='random', print_joints=False, gender='random') self.robot_lower_limits = self.robot_lower_limits[ self.robot_right_arm_joint_indices] self.robot_upper_limits = self.robot_upper_limits[ self.robot_right_arm_joint_indices] self.reset_robot_joints() # # if self.robot_type == 'jaco': # wheelchair_pos, wheelchair_orient = p.getBasePositionAndOrientation(self.wheelchair, # physicsClientId=self.id) # p.resetBasePositionAndOrientation(self.robot, np.array(wheelchair_pos) + np.array([-0.35, -0.3, 0.3]), # p.getQuaternionFromEuler([0, 0, -np.pi / 2.0], physicsClientId=self.id), # physicsClientId=self.id) # base_pos, base_orient = p.getBasePositionAndOrientation(self.robot, physicsClientId=self.id) joints_positions = [(6, np.deg2rad(-90)), (16, np.deg2rad(-90)), (28, np.deg2rad(-90)), (31, np.deg2rad(80)), (35, np.deg2rad(-90)), (38, np.deg2rad(80))] joints_positions += [ (21, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))), (22, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))), (23, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))) ] self.human_controllable_joint_indices = [20, 21, 22, 23] self.world_creation.setup_human_joints( self.human, joints_positions, self.human_controllable_joint_indices if (self.human_control or self.world_creation.human_impairment == 'tremor') else [], use_static_joints=True, human_reactive_force=None) p.resetBasePositionAndOrientation( self.human, [0, 0.03, 0.89 if self.gender == 'male' else 0.86], [0, 0, 0, 1], physicsClientId=self.id) human_joint_states = p.getJointStates( self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id) self.target_human_joint_positions = np.array( [x[0] for x in human_joint_states]) self.human_lower_limits = self.human_lower_limits[ self.human_controllable_joint_indices] self.human_upper_limits = self.human_upper_limits[ self.human_controllable_joint_indices] # Place a bowl of food on a table self.table = p.loadURDF(os.path.join(self.world_creation.directory, 'table', 'table_tall.urdf'), basePosition=[0.35, -0.9, 0], baseOrientation=p.getQuaternionFromEuler( [0, 0, 0], physicsClientId=self.id), physicsClientId=self.id) # MOUTH SIM self.mouth_pos = [-0, -0.15, 1.2] self.mouth_orient = p.getQuaternionFromEuler( [np.pi / 2, np.pi / 2, -np.pi / 2], physicsClientId=self.id) self.mouth = p.loadURDF(os.path.join(self.world_creation.directory, 'mouth', 'hole.urdf'), useFixedBase=True, basePosition=self.mouth_pos, baseOrientation=self.mouth_orient, flags=p.URDF_USE_SELF_COLLISION, physicsClientId=self.id) sph_vis = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.005, rgbaColor=[0, 1, 0, 1], physicsClientId=self.id) self.mouth_center_vis = p.createMultiBody(baseMass=0.0, baseCollisionShapeIndex=-1, baseVisualShapeIndex=sph_vis, basePosition=self.mouth_pos, useMaximalCoordinates=False, physicsClientId=self.id) self.batch_ray_offsets = [[0.08, 0, 0], [-0.08, 0, 0], [0, 0, 0.05], [0, 0, -0.05]] self.ray_markers = [ p.createMultiBody(baseMass=0.0, baseCollisionShapeIndex=-1, baseVisualShapeIndex=sph_vis, basePosition=self.mouth_pos, useMaximalCoordinates=False, physicsClientId=self.id) for i in range(4) ] self.ray_markers_end = [ p.createMultiBody(baseMass=0.0, baseCollisionShapeIndex=-1, baseVisualShapeIndex=sph_vis, basePosition=self.mouth_pos, useMaximalCoordinates=False, physicsClientId=self.id) for i in range(4) ] # SCALE = 0.1 # self.mouthVisualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH, # fileName=os.path.join(self.world_creation.directory, 'mouth', 'hole2.obj'), # rgbaColor=[0.8, 0.4, 0, 1], # # specularColor=[0.4, .4, 0], # visualFramePosition=[0, 0, 0], # meshScale=[SCALE] * 3, # physicsClientId=self.id) # self.mouthCollisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, # fileName=os.path.join(self.world_creation.directory, 'mouth', 'hole2.obj'), # collisionFramePosition=[0, 0, 0], # meshScale=[SCALE] * 3, # flags=p.GEOM_FORCE_CONCAVE_TRIMESH, # physicsClientId=self.id) # self.mouth = p.createMultiBody(baseMass=0.1, # baseInertialFramePosition=[0, 0, 0], # baseCollisionShapeIndex=self.mouthCollisionShapeId, # baseVisualShapeIndex=self.mouthVisualShapeId, # basePosition=[0, 0, 0], # useMaximalCoordinates=True, # flags=p.URDF_USE_SELF_COLLISION, # physicsClientId=self.id) p.resetDebugVisualizerCamera(cameraDistance=1.10, cameraYaw=40, cameraPitch=-45, cameraTargetPosition=[-0.2, 0, 0.75], physicsClientId=self.id) # ROBOT STUFF # target_pos = np.array(bowl_pos) + np.array([0, -0.1, 0.4]) + self.np_random.uniform(-0.05, 0.05, size=3) if self.robot_type == 'panda': # target_pos = [0.2, -0.7, 1.4] # target_orient = [-0.7350878727462702, -8.032928869253401e-09, 7.4087721728719465e-09, # 0.6779718425874067] # target_orient = [0.4902888273008801, -0.46462044731135954, -0.5293302738768326, -0.5133752690981496] # target_orient = p.getQuaternionFromEuler(np.array([0, 0, np.pi / 2.0]), physicsClientId=self.id) # self.util.ik_random_restarts(self.robot, 8, target_pos, target_orient, self.world_creation, # self.robot_right_arm_joint_indices, self.robot_lower_limits, # self.robot_upper_limits, # ik_indices=range(len(self.robot_right_arm_joint_indices)), max_iterations=1000, # max_ik_random_restarts=40, random_restart_threshold=0.01, step_sim=True, # check_env_collisions=True) # # positions = [0.42092164, -0.92326318, -0.33538581, -2.65185322, 1.40763901, 1.81818155, 0.58610855, 0, 0, # 0.02, 0.02] # 11 # positions = [0.4721324, -0.87257245, -0.2123101, -2.44757844, 1.47352227, 1.90545005, 0.75139663] positions = [ 0.44594466, -0.58616227, -0.78082232, -2.59866731, 1.15649738, 1.54176148, 0.15145287 ] for idx in range(len(positions)): p.resetJointState(self.robot, idx, positions[idx]) self.world_creation.set_gripper_open_position(self.robot, position=0.00, left=False, set_instantly=True) self.drop_fork = self.world_creation.init_tool( self.robot, mesh_scale=[0.08] * 3, pos_offset=[0, 0, 0.08], # fork: [0, -0.02, 0.16], orient_offset=p.getQuaternionFromEuler( [-0, -np.pi, 0], physicsClientId=self.id), left=False, maximal=False) else: raise NotImplementedError # LOAD FOOD ITEM self.food_type = np.random.randint(0, len(self.foods), dtype=int) self.food_scale = np.random.uniform( self.foods_scale_range[self.food_type][0], self.foods_scale_range[self.food_type][1]) print("LOADING food file: %s with scale %.2f" % (self.foods[self.food_type], self.food_scale)) self.child_offset = self.food_scale * self.food_offsets[ self.food_type] # [-0.004, -0.0065] self.food_orient_eul = np.random.rand(3) * 2 * np.pi self.food_orient_quat = p.getQuaternionFromEuler( self.food_orient_eul, physicsClientId=self.id) mesh_scale = np.array( [self.food_base_scale[self.food_type]] * 3) * self.food_scale food_visual = p.createVisualShape( shapeType=p.GEOM_MESH, fileName=os.path.join(self.world_creation.directory, 'food_items', self.foods[self.food_type] + ".obj"), rgbaColor=[1.0, 1.0, 1.0, 1.0], meshScale=mesh_scale, physicsClientId=self.id) food_collision = p.createCollisionShape( shapeType=p.GEOM_MESH, fileName=os.path.join(self.world_creation.directory, 'food_items', self.foods[self.food_type] + "_vhacd.obj"), meshScale=mesh_scale, physicsClientId=self.id) self.food_item = p.createMultiBody( baseMass=0.012, baseCollisionShapeIndex=food_collision, baseVisualShapeIndex=food_visual, basePosition=[0, 0, 0], useMaximalCoordinates=False, physicsClientId=self.id) p.changeVisualShape( self.food_item, -1, textureUniqueId=p.loadTexture( os.path.join(self.world_creation.directory, 'food_items', self.foods[self.food_type] + ".png"))) # Disable collisions between the tool and food item for ti in list( range(p.getNumJoints(self.drop_fork, physicsClientId=self.id))) + [-1]: for tj in list( range( p.getNumJoints(self.food_item, physicsClientId=self.id))) + [-1]: p.setCollisionFilterPair(self.drop_fork, self.food_item, ti, tj, False, physicsClientId=self.id) # Create constraint that keeps the food item in the tool constraint = p.createConstraint( self.drop_fork, -1, self.food_item, -1, p.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=[0, 0, -0.025], childFramePosition=self.child_offset, parentFrameOrientation=self.food_orient_quat, childFrameOrientation=[0, 0, 0, 1], physicsClientId=self.id) p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id) # p.resetBasePositionAndOrientation(self.bowl, bowl_pos, p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id) p.setGravity(0, 0, -9.81, physicsClientId=self.id) p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id) p.setGravity(0, 0, 0, body=self.human, physicsClientId=self.id) p.setPhysicsEngineParameter(numSubSteps=5, numSolverIterations=10, physicsClientId=self.id) # Enable rendering p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id) # init_pos = [0.0, 0.0, 0.0, -2 * np.pi / 4, 0.0, np.pi / 2, np.pi / 4, 0.0, 0.0, 0.05, 0.05] # self._reset_robot(init_pos) # initialize images for i in range(10): p.stepSimulation() ee_pos, ee_orient = p.getBasePositionAndOrientation(self.food_item) self.viewMat1 = p.computeViewMatrixFromYawPitchRoll( ee_pos, self.camdist, self.yaw, self.pitch, self.roll, 2, self.id) self.viewMat2 = p.computeViewMatrixFromYawPitchRoll( ee_pos, self.camdist, -self.yaw, self.pitch, self.roll, 2, self.id) self.projMat = p.computeProjectionMatrixFOV( self.fov, self.img_width / self.img_height, self.near, self.far) images1 = p.getCameraImage(self.img_width, self.img_height, self.viewMat1, self.projMat, shadow=True, renderer=p.ER_BULLET_HARDWARE_OPENGL, physicsClientId=self.id) images2 = p.getCameraImage(self.img_width, self.img_height, self.viewMat2, self.projMat, shadow=True, renderer=p.ER_BULLET_HARDWARE_OPENGL, physicsClientId=self.id) self.rgb_opengl1 = np.reshape( images1[2], (self.img_width, self.img_height, 4)) * 1. / 255. depth_buffer_opengl = np.reshape(images1[3], [self.img_width, self.img_height]) self.depth_opengl1 = self.far * self.near / ( self.far - (self.far - self.near) * depth_buffer_opengl) self.rgb_opengl2 = np.reshape( images2[2], (self.img_width, self.img_height, 4)) * 1. / 255. depth_buffer_opengl = np.reshape(images2[3], [self.img_width, self.img_height]) self.depth_opengl2 = self.far * self.near / ( self.far - (self.far - self.near) * depth_buffer_opengl) if self.gui: # TODO: do we need to update this every step? prob not self.im1.set_data(self.rgb_opengl1) self.im2.set_data(self.rgb_opengl2) self.im1_d.set_data(self.depth_opengl1) self.im2_d.set_data(self.depth_opengl2) print(np.min(self.depth_opengl1), np.max(self.depth_opengl1)) self.fig.canvas.draw() return self._get_obs(ret_images=ret_images, ret_dict=ret_dict)
def LoadModel(self, model_name, model_path, start_pos = [0, 0, 0], start_orientation = [0, 0, 0]): start_orientation = p.getQuaternionFromEuler(start_orientation) self.model_name_id_dict[model_name] = p.loadURDF(model_path, start_pos, start_orientation, useFixedBase=1)
import pybullet as p import time import pybullet_data # Setup environment physicsClient = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) planeId = p.loadURDF("plane.urdf") p.setAdditionalSearchPath("/home/oreo/Documents/oreo_sim/four_bar_link") p.setRealTimeSimulation(0) startPos = [0, 0, 1] startOrn = p.getQuaternionFromEuler([0, 0, 0]) linkage = p.loadURDF("four_bar_link.urdf", startPos, startOrn, useFixedBase=1) #Set joint control here numJoints = p.getNumJoints(linkage) jointInfoList = {} for i in range(numJoints): jointInfo = p.getJointInfo(linkage, i) jointInfoList[jointInfo[1].decode('UTF-8')] = jointInfo[0] print(jointInfo[0]) # Set some positions #p.resetJointState(linkage, jointInfoList['joint1'], -1.57) #p.resetJointState(linkage, jointInfoList['joint2'], -1.57) #p.resetJointState(linkage, jointInfoList['joint3'], -1.57) p.setGravity(0, 0, -9.8) # Constraint to close the kinematic loop cid = p.createConstraint(linkage, jointInfoList['dummy_joint'], linkage,
def create_smplx_body(self, directory, id, np_random, gender='female', height=None, body_shape=None, joint_angles=[], position=[0, 0, 0], orientation=[0, 0, 0], body_pose=None): # Choose gender self.gender = gender if self.gender not in ['male', 'female']: self.gender = np_random.choice(['male', 'female']) # Create SMPL-X model model_folder = os.path.join(directory, 'smpl_models') model = smplx.create(model_folder, model_type='smplx', gender=self.gender) # Define body shape if type(body_shape) == str: params_filename = os.path.join(model_folder, 'human_params', body_shape) with open(params_filename, 'rb') as f: params = pickle.load(f) betas = torch.Tensor(params['betas']) elif body_shape is None: betas = torch.Tensor( np_random.uniform(-1, 5, (1, self.num_body_shape))) else: betas = torch.Tensor(body_shape) # betas = torch.Tensor(np.zeros((1, 10))) # betas = torch.Tensor(np.zeros((1, 10))) # Set human body pose if body_pose is None: body_pose = np.zeros((1, model.NUM_BODY_JOINTS * 3)) for joint_index, angle in joint_angles: body_pose[0, joint_index] = np.deg2rad(angle) # Generate standing human mesh and determine default height of the mesh output = model(betas=betas, body_pose=torch.Tensor( np.zeros((1, model.NUM_BODY_JOINTS * 3))), return_verts=True) vertices = output.vertices.detach().cpu().numpy().squeeze() out_mesh = trimesh.Trimesh(vertices, model.faces) rot = trimesh.transformations.rotation_matrix(np.deg2rad(90), [1, 0, 0]) out_mesh.apply_transform(rot) # Find indices for right arm vertices and save to file # self.vert_indices = np.where(np.logical_and(np.logical_and(vertices[:, 0] < -0.17, vertices[:, 1] > -0.1), vertices[:, 0] > -0.64)) # self.vert_indices = np.where(np.logical_and(np.logical_and(np.logical_and(np.logical_and(vertices[:, 1] > -0.4, vertices[:, 1] < -0.37), vertices[:, 0] < 0.02), vertices[:, 0] > -0.02), vertices[:, 2] < 0)) # np.savetxt('right_arm_vertex_indices.csv', self.vert_indices, delimiter=',', fm1='%d') # Generate human mesh with correct height scaling height_scale = height / out_mesh.extents[ -1] if height is not None else 1.0 # print('Scale:', height_scale, '=', height, '/', out_mesh.extents[-1]) output = model(betas=betas, body_pose=torch.Tensor(body_pose), return_verts=True) vertices = output.vertices.detach().cpu().numpy().squeeze() joints = output.joints.detach().cpu().numpy().squeeze() # Scale vertices and rotate orient_quat = p.getQuaternionFromEuler(orientation, physicsClientId=id) vertices = vertices * height_scale vertices = vertices.dot( R.from_euler('x', -90, degrees=True).as_matrix()) vertices = vertices.dot(R.from_quat(orient_quat).as_matrix()) joints = joints * height_scale joints = joints.dot(R.from_euler('x', -90, degrees=True).as_matrix()) joints = joints.dot(R.from_quat(orient_quat).as_matrix()) out_mesh = trimesh.Trimesh(vertices, model.faces) # scale = trimesh.transformations.scale_matrix(height_scale, [0, 0, 0]) # out_mesh.apply_transform(scale) # rot = trimesh.transformations.rotation_matrix(np.deg2rad(90), [1, 0, 0]) # out_mesh.apply_transform(rot) return out_mesh, vertices, joints
def move_in_xyz(environment, arm, abs_rel='abs'): motorsIds = [] dv = 0.01 abs_distance = 1.0 # environment._arm.resetJointPoses() observation = environment._arm.getObservation() xyz = observation[-7:-4] ori = p.getEulerFromQuaternion(observation[-4:]) if abs_rel == 'abs': if arm == 'ur5': xin = xyz[0] yin = xyz[1] zin = xyz[2] rin = -0.6943 #ori[0] pitchin = 1.587 #ori[1] yawin = -0.694 # ori[2] #rin = 1.57# #pitchin = -0.78 # #yawin = -0.78 print(pitchin) print(rin) print(yawin) motorsIds.append( environment._p.addUserDebugParameter("X", -abs_distance, abs_distance, xin)) motorsIds.append( environment._p.addUserDebugParameter("Y", -abs_distance, abs_distance, yin)) motorsIds.append( environment._p.addUserDebugParameter("Z", -abs_distance, abs_distance, zin)) motorsIds.append( environment._p.addUserDebugParameter( "roll", -math.pi, math.pi, rin, )) motorsIds.append( environment._p.addUserDebugParameter("pitch", -math.pi, math.pi, pitchin)) motorsIds.append( environment._p.addUserDebugParameter("yaw", -math.pi, math.pi, yawin)) motorsIds.append( environment._p.addUserDebugParameter("fingerAngle", 0, 1.5, .3)) grasp_switch = environment._p.addUserDebugParameter("grasp", 0, 2, 0) throw_switch = environment._p.addUserDebugParameter("throw", 0, 2, 0) throw_range = environment._p.addUserDebugParameter("range", 0, 5, 1) done = False grasped = 0 lifted = 0 throw_timeout = 0 while (not done): action = [] # get state observation = environment._arm.getObservation() grip_img, rgb = gripper_camera(observation) img = process_image(rgb) save_image(img) xyz = observation[-7:-4] ori = p.getEulerFromQuaternion(observation[-4:]) grip = observation[6] print(grip) cube_pos = get_scene_observation(environment.objects)[0:3] if cube_pos[2] < 0.0: #reset cube if it falls below table. p.resetBasePositionAndOrientation(environment.objects[0][0], [0, 0, 0], [0.0, 0.0, 0.000000, 1.0]) print(grasped, lifted) if xyz[2] < 0.15: print('reset lift') lifted = 0 if not (((xyz[0] - cube_pos[0]) < 0.02) or ((xyz[1] - cube_pos[1]) < 0.02) or ((xyz[2] - cube_pos[2]) < 0.15)): grasped = 0 for motorId in motorsIds: # print(environment._p.readUserDebugParameter(motorId)) action.append(environment._p.readUserDebugParameter(motorId)) update_camera(environment) #environment._p.addUserDebugLine(environment._arm.endEffectorPos, [0, 0, 0], [1, 0, 0], 5) # action is xyz positon, orietnation quaternion, gripper closedness. action = action[0:3] + list(p.getQuaternionFromEuler( action[3:6])) + [action[6]] if environment._p.readUserDebugParameter(throw_switch) > 1: action, grasped, lifted, throw_timeout = throw( environment, arm, action, xyz, ori, grip, cube_pos, grasped, lifted, throw_timeout, img, environment._p.readUserDebugParameter(throw_range)) elif environment._p.readUserDebugParameter(grasp_switch) > 1: action, grasped, lifted = grasp_primitive(environment, arm, action, xyz, ori, grip, cube_pos, grasped, lifted, img) state, motor_action, reward, done, info = environment.step_to( action, abs_rel) obs = environment.getSceneObservation()
TORSO_JOINT_NAME = 'torso_lift_joint' #Setup node pub = rospy.Publisher('pybullet_cmds', uc_msg, queue_size=0) rospy.init_node("pybullet_test") rospy.loginfo("testing pybullet configurations") #Setup controller uc = Uber() uc.command_joint_pose('l', [0]*7, time=2, blocking=True) #Setup sim physicsClient = connect(use_gui=True) #or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) pr2_start_orientation = p.getQuaternionFromEuler([0,0,0]) pr2_start_pose = [-.80*k,0,0] pr2 = p.loadURDF("/home/demo/nishad/pddlstream/examples/pybullet/utils/models/pr2_description/pr2.urdf", pr2_start_pose, pr2_start_orientation, useFixedBase=True, globalScaling = 1 ) #The goal here is to get the arm positions of the actual robot using the uber controller #and simulate them in pybullet print str(uc.get_head_pose()) + ',' #Left Manipulator left_joints = [joint_from_name(pr2, name) for name in ARM_JOINT_NAMES['left']] l_joint_poses = uc.get_joint_positions('l') #Right Manipulator right_joints = [joint_from_name(pr2, name) for name in ARM_JOINT_NAMES['right']] r_joint_poses = uc.get_joint_positions('r')
def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0): #assume link[0] is base if (len(self.urdfLinks)==0): return -1 base = self.urdfLinks[0] #v.tmp_collision_shape_ids=[] baseMass = base.urdf_inertial.mass print("baseMass=",baseMass) baseCollisionShapeIndex = -1 baseShapeTypeArray=[] baseRadiusArray=[] baseHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] meshScaleArray=[] basePositionsArray=[] baseOrientationsArray=[] for v in base.urdf_collision_shapes: print("base v.origin_xyz=",v.origin_xyz) print("base v.origin_rpy=",v.origin_rpy) shapeType = v.geom_type baseShapeTypeArray.append(shapeType) baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) baseRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) meshScaleArray.append(v.geom_meshscale) basePositionsArray.append(v.origin_xyz) print("v.origin_rpy=",v.origin_rpy) orn=p.getQuaternionFromEuler(v.origin_rpy) baseOrientationsArray.append(orn) print("baseHalfExtentsArray=",baseHalfExtentsArray) if (len(baseShapeTypeArray)): baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray, radii=baseRadiusArray, halfExtents=baseHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=meshScaleArray, collisionFramePositions=basePositionsArray, collisionFrameOrientations=baseOrientationsArray, physicsClientId=physicsClientId) print("baseCollisionShapeIndex=",baseCollisionShapeIndex) linkMasses=[] linkCollisionShapeIndices=[] linkVisualShapeIndices=[] linkPositions=[] linkOrientations=[] linkMeshScaleArray=[] linkInertialFramePositions=[] linkInertialFrameOrientations=[] linkParentIndices=[] linkJointTypes=[] linkJointAxis=[] for joint in self.urdfJoints: link = joint.link linkMass = link.urdf_inertial.mass print("linkMass=",linkMass) linkCollisionShapeIndex=-1 linkVisualShapeIndex=-1 linkPosition=[0,0,0] linkOrientation=[0,0,0] linkInertialFramePosition=[0,0,0] linkInertialFrameOrientation=[0,0,0] linkParentIndex=self.linkNameToIndex[joint.parent_name] print("parentId=",linkParentIndex) linkJointType=joint.joint_type print("linkJointType=",linkJointType, self.jointTypeToName[linkJointType] ) linkJointAx = joint.joint_axis_xyz linkShapeTypeArray=[] linkRadiusArray=[] linkHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] linkPositionsArray=[] linkOrientationsArray=[] for v in link.urdf_collision_shapes: print("link v.origin_xyz=",v.origin_xyz) print("link v.origin_rpy=",v.origin_rpy) shapeType = v.geom_type linkShapeTypeArray.append(shapeType) linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) linkRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) linkMeshScaleArray.append(v.geom_meshscale) linkPositionsArray.append(v.origin_xyz) linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy)) print("linkHalfExtentsArray=",linkHalfExtentsArray) if (len(linkShapeTypeArray)): linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray, radii=linkRadiusArray, halfExtents=linkHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=linkMeshScaleArray, collisionFramePositions=linkPositionsArray, collisionFrameOrientations=linkOrientationsArray, physicsClientId=physicsClientId) linkMasses.append(linkMass) linkCollisionShapeIndices.append(linkCollisionShapeIndex) linkVisualShapeIndices.append(linkVisualShapeIndex) linkPositions.append(joint.joint_origin_xyz) linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy)) linkInertialFramePositions.append(link.urdf_inertial.origin_xyz) linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy)) linkParentIndices.append(linkParentIndex) linkJointTypes.append(joint.joint_type) linkJointAxis.append(joint.joint_axis_xyz) print("\n\n\nlinkMasses=",linkMasses) obUid = p.createMultiBody(baseMass,\ baseCollisionShapeIndex= baseCollisionShapeIndex, basePosition=basePosition, baseInertialFramePosition=base.urdf_inertial.origin_xyz, baseInertialFrameOrientation=base.urdf_inertial.origin_rpy, linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=linkParentIndices, linkJointTypes=linkJointTypes, linkJointAxis=linkJointAxis, physicsClientId=physicsClientId) return obUid
def configure_simulation(dt, enableGUI): global jointTorques # Load robot robot = loadTalos() robot.initDisplay(loadModel=True) rmodel = robot.model # Defining the initial state of the robot q0 = robot.model.referenceConfigurations['half_sitting'].copy() q0[2] = 1.2 # Note that pinocchio's index is as follow: left leg (7), right leg, torso(2), left arm(8), right arm, head(2) v0 = pinocchio.utils.zero(robot.model.nv) nq = robot.model.nq nv = robot.model.nv na = nq - 7 # actual activated joint # Start the client for PyBullet if enableGUI: physicsClient = p.connect(p.GUI) else: physicsClient = p.connect(p.DIRECT) # noqa # p.GUI for graphical version # p.DIRECT for non-graphical version # Set gravity (disabled by default) p.setGravity(0, 0, -9.81) # Load horizontal plane for PyBullet p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") # Load the robot for PyBullet robotStartPos = q0[:3] robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) p.setAdditionalSearchPath( "/opt/openrobots/share/example-robot-data/robots/talos_data/robots") robotId = p.loadURDF("talos_reduced.urdf", robotStartPos, robotStartOrientation) # Set time step of the simulation # dt = 0.001 p.setTimeStep(dt) # realTimeSimulation = True # If True then we will sleep in the main loop to have a frequency of 1/dt # set Initial Posture revoluteJointIndices = np.concatenate( (np.arange(45, 51), np.arange(52, 58), np.arange( 0, 2), np.arange(11, 19), np.arange(28, 36), np.arange(3, 5)), axis=None) for m in range(na): p.resetJointState(robotId, revoluteJointIndices[m], q0[m + 7]) # Disable default motor control for revolute joints p.setJointMotorControlArray( robotId, jointIndices=revoluteJointIndices, controlMode=p.VELOCITY_CONTROL, targetVelocities=[0.0 for m in revoluteJointIndices], forces=[0.0 for m in revoluteJointIndices]) # Enable torque control for revolute joints jointTorques = [0.0 for m in revoluteJointIndices] p.setJointMotorControlArray(robotId, revoluteJointIndices, controlMode=p.TORQUE_CONTROL, forces=jointTorques) # Compute one step of simulation for initialization p.stepSimulation() return robotId, robot, revoluteJointIndices
#print("duration=",frameData[0]) #print(pos=[frameData]) basePos1Start = [frameData[1],frameData[2],frameData[3]] basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]] basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])] baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]] baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]] baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction) #pre-rotate to make z-up if (useZUp): y2zPos=[0,0,0.0] y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn) y2zPos=[0,2,0.0] y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn) chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]] chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction) neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]] neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
def reset(self): self.setup_timing() self.task_success = 0 self.forearm_in_sleeve = False self.upperarm_in_sleeve = False self.human, self.wheelchair, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world(furniture_type='wheelchair', static_human_base=True, human_impairment='random', print_joints=False, gender='random') self.robot_lower_limits = self.robot_lower_limits[self.robot_left_arm_joint_indices] self.robot_upper_limits = self.robot_upper_limits[self.robot_left_arm_joint_indices] self.reset_robot_joints() if self.robot_type == 'jaco': wheelchair_pos, wheelchair_orient = p.getBasePositionAndOrientation(self.wheelchair, physicsClientId=self.id) p.resetBasePositionAndOrientation(self.robot, np.array(wheelchair_pos) + np.array([0.35, -0.3, 0.3]), p.getQuaternionFromEuler([0, 0, 0], physicsClientId=self.id), physicsClientId=self.id) joints_positions = [(6, np.deg2rad(-90)), (13, np.deg2rad(-80)), (16, np.deg2rad(-90)), (28, np.deg2rad(-90)), (31, np.deg2rad(80)), (35, np.deg2rad(-90)), (38, np.deg2rad(80))] self.human_controllable_joint_indices = [10, 11, 12, 13, 14, 15, 16, 17, 18, 19] self.world_creation.setup_human_joints(self.human, joints_positions, self.human_controllable_joint_indices if (self.human_control or self.world_creation.human_impairment == 'tremor') else [], use_static_joints=True, human_reactive_force=None) p.resetBasePositionAndOrientation(self.human, [0, 0.03, 0.89 if self.gender == 'male' else 0.86], [0, 0, 0, 1], physicsClientId=self.id) human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id) self.target_human_joint_positions = np.array([x[0] for x in human_joint_states]) self.human_lower_limits = self.human_lower_limits[self.human_controllable_joint_indices] self.human_upper_limits = self.human_upper_limits[self.human_controllable_joint_indices] shoulder_pos, shoulder_orient = p.getLinkState(self.human, 15, computeForwardKinematics=True, physicsClientId=self.id)[:2] elbow_pos, elbow_orient = p.getLinkState(self.human, 17, computeForwardKinematics=True, physicsClientId=self.id)[:2] wrist_pos, wrist_orient = p.getLinkState(self.human, 19, computeForwardKinematics=True, physicsClientId=self.id)[:2] target_pos = np.array([-0.85, -0.4, 0]) + np.array([1.85, 0.6, 0]) + np.array([-0.55, -0.5, 1.2]) + self.np_random.uniform(-0.05, 0.05, size=3) offset = np.array([0, 0, 0.1]) if self.robot_type == 'pr2': target_orient = p.getQuaternionFromEuler([0, 0, np.pi], physicsClientId=self.id) target_orient_shoulder = p.getQuaternionFromEuler([0, 0, np.pi*3/2.0], physicsClientId=self.id) self.position_robot_toc(self.robot, 76, [(target_pos, target_orient)], [(shoulder_pos+offset, target_orient_shoulder), (elbow_pos+offset, target_orient), (wrist_pos+offset, target_orient)], self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=range(29, 29+7), pos_offset=np.array([1.7, 0.7, 0]), base_euler_orient=[0, 0, np.pi], right_side=False, max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) elif self.robot_type == 'jaco': target_orient = p.getQuaternionFromEuler(np.array([0, -np.pi/2.0, 0]), physicsClientId=self.id) target_joint_positions = self.util.ik_random_restarts(self.robot, 8, target_pos, target_orient, self.world_creation, self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=[0, 1, 2, 3, 4, 5, 6], max_iterations=1000, max_ik_random_restarts=40, random_restart_threshold=0.03, step_sim=True) self.world_creation.set_gripper_open_position(self.robot, position=1.33, left=False, set_instantly=True) else: target_orient = p.getQuaternionFromEuler([0, -np.pi/2.0, 0], physicsClientId=self.id) target_orient_shoulder = p.getQuaternionFromEuler([np.pi/2.0, -np.pi/2.0, 0], physicsClientId=self.id) if self.robot_type == 'baxter': self.position_robot_toc(self.robot, 48, [(target_pos, target_orient)], [(shoulder_pos+offset, target_orient_shoulder), (elbow_pos+offset, target_orient), (wrist_pos+offset, target_orient)], self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=range(10, 17), pos_offset=np.array([1.7, 0.7, 0.975]), base_euler_orient=[0, 0, np.pi], right_side=False, max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) else: self.position_robot_toc(self.robot, 19, [(target_pos, target_orient)], [(shoulder_pos+offset, target_orient_shoulder), (elbow_pos+offset, target_orient), (wrist_pos+offset, target_orient)], self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=[0, 2, 3, 4, 5, 6, 7], pos_offset=np.array([1.8, 0.7, 0.975]), base_euler_orient=[0, 0, np.pi], right_side=False, max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) if self.human_control or self.world_creation.human_impairment == 'tremor': human_len = len(self.human_controllable_joint_indices) human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id) human_joint_positions = np.array([x[0] for x in human_joint_states]) p.setJointMotorControlArray(self.human, jointIndices=self.human_controllable_joint_indices, controlMode=p.POSITION_CONTROL, targetPositions=human_joint_positions, positionGains=np.array([0.005]*human_len), forces=[1]*human_len, physicsClientId=self.id) state = p.getLinkState(self.robot, 76 if self.robot_type=='pr2' else 19 if self.robot_type=='sawyer' else 48 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id) self.start_ee_pos = np.array(state[0]) self.start_ee_orient = np.array(state[1]) # Quaternions self.cloth_orig_pos = np.array([0.34658437, -0.30296362, 1.20023387]) self.cloth_offset = self.start_ee_pos - self.cloth_orig_pos # Set up gripper - cloth constraint gripper_visual = p.createVisualShape(p.GEOM_SPHERE, radius=0.01, rgbaColor=[0, 0, 0, 0], physicsClientId=self.id) gripper_collision = p.createCollisionShape(p.GEOM_SPHERE, radius=0.0001, physicsClientId=self.id) self.cloth_attachment = p.createMultiBody(baseMass=0, baseVisualShapeIndex=gripper_visual, baseCollisionShapeIndex=gripper_collision, basePosition=self.start_ee_pos, useMaximalCoordinates=1, physicsClientId=self.id) # Load cloth self.cloth = p.loadCloth(os.path.join(self.world_creation.directory, 'clothing', 'hospitalgown_reduced.obj'), scale=1.4, mass=0.23, position=np.array([0.02, -0.38, 0.83]) + self.cloth_offset/1.4, orientation=p.getQuaternionFromEuler([0, 0, np.pi], physicsClientId=self.id), bodyAnchorId=self.cloth_attachment, anchors=[2087, 3879, 3681, 3682, 2086, 2041, 987, 2042, 2088, 1647, 2332], collisionMargin=0.04, rgbaColor=np.array([139./256., 195./256., 74./256., 0.6]), rgbaLineColor=np.array([197./256., 225./256., 165./256., 1]), physicsClientId=self.id) p.clothParams(self.cloth, kLST=0.05, kAST=1.0, kVST=1.0, kDP=0.001, kDG=10, kDF=0.25, kCHR=1.0, kKHR=1.0, kAHR=0.5, piterations=5, physicsClientId=self.id) self.triangle1_point_indices = [621, 37, 1008] self.triangle2_point_indices = [130, 3908, 2358] # double m_kLST; // Material: Linear stiffness coefficient [0,1] # double m_kAST; // Material: Area/Angular stiffness coefficient [0,1] # double m_kVST; // Material: Volume stiffness coefficient [0,1] # double m_kVCF; // Velocities correction factor (Baumgarte) # double m_kDP; // Damping coefficient [0,1] # double m_kDG; // Drag coefficient [0,+inf] # double m_kLF; // Lift coefficient [0,+inf] # double m_kPR; // Pressure coefficient [-inf,+inf] # double m_kVC; // Volume conversation coefficient [0,+inf] # double m_kDF; // Dynamic friction coefficient [0,1] # double m_kMT; // Pose matching coefficient [0,1] # double m_kCHR; // Rigid contacts hardness [0,1] # double m_kKHR; // Kinetic contacts hardness [0,1] # double m_kSHR; // Soft contacts hardness [0,1] # double m_kAHR; // Anchors hardness [0,1] # int m_viterations; // Velocities solver iterations # int m_piterations; // Positions solver iterations # int m_diterations; // Drift solver iterations p.setGravity(0, 0, -9.81/2, physicsClientId=self.id) # Let the cloth settle more gently p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id) p.setGravity(0, 0, -1, body=self.human, physicsClientId=self.id) p.setGravity(0, 0, 0, body=self.cloth_attachment, physicsClientId=self.id) p.setPhysicsEngineParameter(numSubSteps=4, physicsClientId=self.id) # Enable rendering p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id) # Wait for the cloth to settle for _ in range(200): # Force the cloth attachment to stay at the end effector p.resetBasePositionAndOrientation(self.cloth_attachment, self.start_ee_pos, [0, 0, 0, 1], physicsClientId=self.id) p.stepSimulation(physicsClientId=self.id) p.setGravity(0, 0, -9.81, physicsClientId=self.id) return self._get_obs([0], [0, 0])
def applyAction(self, motorCommands): #print ("self.numJoints") #print (self.numJoints) if (self.useInverseKinematics): dx = motorCommands[0] dy = motorCommands[1] dz = motorCommands[2] da = motorCommands[3] fingerAngle = motorCommands[4] state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex) actualEndEffectorPos = state[0] #print("pos[2] (getLinkState(kukaEndEffectorIndex)") #print(actualEndEffectorPos[2]) self.endEffectorPos[0] = self.endEffectorPos[0]+dx if (self.endEffectorPos[0]>0.65): self.endEffectorPos[0]=0.65 if (self.endEffectorPos[0]<0.50): self.endEffectorPos[0]=0.50 self.endEffectorPos[1] = self.endEffectorPos[1]+dy if (self.endEffectorPos[1]<-0.17): self.endEffectorPos[1]=-0.17 if (self.endEffectorPos[1]>0.22): self.endEffectorPos[1]=0.22 #print ("self.endEffectorPos[2]") #print (self.endEffectorPos[2]) #print("actualEndEffectorPos[2]") #print(actualEndEffectorPos[2]) #if (dz<0 or actualEndEffectorPos[2]<0.5): self.endEffectorPos[2] = self.endEffectorPos[2]+dz self.endEffectorAngle = self.endEffectorAngle + da pos = self.endEffectorPos orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw]) if (self.useNullSpace==1): if (self.useOrientation==1): jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp) else: jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp) else: if (self.useOrientation==1): jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd) else: jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos) #print("jointPoses") #print(jointPoses) #print("self.kukaEndEffectorIndex") #print(self.kukaEndEffectorIndex) if (self.useSimulation): for i in range (self.kukaEndEffectorIndex+1): #print(i) p.setJointMotorControl2(bodyUniqueId=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,maxVelocity=self.maxVelocity, positionGain=0.3,velocityGain=1) else: #reset the joint state (ignoring all dynamics, not recommended to use during simulation) for i in range (self.numJoints): p.resetJointState(self.kukaUid,i,jointPoses[i]) #fingers p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce) p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce) p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce) p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce) p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce) else: for action in range (len(motorCommands)): motor = self.motorIndices[action] p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
import pybullet as p from time import sleep physicsClient = p.connect(p.GUI) p.setGravity(0,0,-10) planeId = p.loadURDF("plane.urdf") cubeStartPos = [0,0,1] cubeStartOrientation = p.getQuaternionFromEuler([0,0,0]) boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) useRealTimeSimulation = 0 if (useRealTimeSimulation): p.setRealTimeSimulation(1) while 1: if (useRealTimeSimulation): p.setGravity(0,0,-10) sleep(0.01) # Time in seconds. else: p.stepSimulation()
def transform_orientation(self, orientation): A = np.quaternion(*orientation) B = np.quaternion(*p.getQuaternionFromEuler([0, np.pi / 2, 0])) C = B * A return quaternion.as_float_array(C)
import pybullet as p import time useMaximalCoordinates = 0 p.connect(p.GUI) #p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates) monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH) orn = p.getQuaternionFromEuler([1.5707963,0,0]) p.createMultiBody (0,monastryId, baseOrientation=orn) sphereRadius = 0.05 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) mass = 1 visualShapeId = -1 for i in range (5): for j in range (5): for k in range (5): if (k&2): sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates) else: sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates) p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) p.setGravity(0,0,-10)
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 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
def applyAction(self, motorCommands): # print ("self.numJoints") # print (self.numJoints) if (self.useInverseKinematics): dx = motorCommands[0] dy = motorCommands[1] dz = motorCommands[2] da = motorCommands[3] fingerAngle = motorCommands[4] state = p.getLinkState(self.kukaUid, self.kukaEndEffectorIndex) actualEndEffectorPos = state[0] # print("pos[2] (getLinkState(kukaEndEffectorIndex)") # print(actualEndEffectorPos[2]) self.endEffectorPos[0] = self.endEffectorPos[0] + dx if (self.endEffectorPos[0] > 0.65): self.endEffectorPos[0] = 0.65 if (self.endEffectorPos[0] < 0.50): self.endEffectorPos[0] = 0.50 self.endEffectorPos[1] = self.endEffectorPos[1] + dy if (self.endEffectorPos[1] < -0.17): self.endEffectorPos[1] = -0.17 if (self.endEffectorPos[1] > 0.22): self.endEffectorPos[1] = 0.22 # print ("self.endEffectorPos[2]") # print (self.endEffectorPos[2]) # print("actualEndEffectorPos[2]") # print(actualEndEffectorPos[2]) # if (dz<0 or actualEndEffectorPos[2]<0.5): self.endEffectorPos[2] = self.endEffectorPos[2] + dz self.endEffectorAngle = self.endEffectorAngle + da pos = self.endEffectorPos orn = p.getQuaternionFromEuler([0, -math.pi, 0]) # -math.pi,yaw]) if (self.useNullSpace == 1): if (self.useOrientation == 1): jointPoses = p.calculateInverseKinematics( self.kukaUid, self.kukaEndEffectorIndex, pos, orn, self.ll, self.ul, self.jr, self.rp) else: jointPoses = p.calculateInverseKinematics( self.kukaUid, self.kukaEndEffectorIndex, pos, lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp) else: if (self.useOrientation == 1): jointPoses = p.calculateInverseKinematics( self.kukaUid, self.kukaEndEffectorIndex, pos, orn, jointDamping=self.jd) # todo make damping list bigger else: jointPoses = p.calculateInverseKinematics( self.kukaUid, self.kukaEndEffectorIndex, pos) # print("jointPoses") # print(jointPoses) # print("self.kukaEndEffectorIndex") # print(self.kukaEndEffectorIndex) if (self.useSimulation): for i in range(self.kukaEndEffectorIndex + 1): # print(i) p.setJointMotorControl2(bodyUniqueId=self.kukaUid, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=jointPoses[i], targetVelocity=0, force=self.maxForce, maxVelocity=self.maxVelocity, positionGain=0.3, velocityGain=1) else: # reset the joint state (ignoring all dynamics, not recommended to use during simulation) for i in range(self.numJoints): p.resetJointState(self.kukaUid, i, jointPoses[i]) # fingers p.setJointMotorControl2(self.kukaUid, 7, p.POSITION_CONTROL, targetPosition=self.endEffectorAngle, force=self.maxForce) p.setJointMotorControl2(self.kukaUid, 8, p.POSITION_CONTROL, targetPosition=-fingerAngle, force=self.fingerAForce) p.setJointMotorControl2(self.kukaUid, 11, p.POSITION_CONTROL, targetPosition=fingerAngle, force=self.fingerBForce) p.setJointMotorControl2(self.kukaUid, 10, p.POSITION_CONTROL, targetPosition=0, force=self.fingerTipForce) p.setJointMotorControl2(self.kukaUid, 13, p.POSITION_CONTROL, targetPosition=0, force=self.fingerTipForce) else: for action in range(len(motorCommands)): motor = self.motorIndices[action] p.setJointMotorControl2(self.kukaUid, motor, p.POSITION_CONTROL, targetPosition=motorCommands[action], force=self.maxForce) targetVelocity = 4 maxForce = 2.5 # maxForce = 0 # targetVelocity = 0 # maxForce = 25 all_wheels = [20, 21] all_wheels = [15, 16] # import pdb; # pdb.set_trace() for wheel in all_wheels: p.setJointMotorControl2(self.kukaUid, wheel, p.VELOCITY_CONTROL, targetVelocity=targetVelocity, force=maxForce)
def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0): #assume link[0] is base if (len(self.urdfLinks)==0): return -1 base = self.urdfLinks[0] #v.tmp_collision_shape_ids=[] baseMass = base.urdf_inertial.mass baseCollisionShapeIndex = -1 baseShapeTypeArray=[] baseRadiusArray=[] baseHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] meshScaleArray=[] basePositionsArray=[] baseOrientationsArray=[] for v in base.urdf_collision_shapes: shapeType = v.geom_type baseShapeTypeArray.append(shapeType) baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) baseRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) meshScaleArray.append(v.geom_meshscale) basePositionsArray.append(v.origin_xyz) orn=p.getQuaternionFromEuler(v.origin_rpy) baseOrientationsArray.append(orn) if (len(baseShapeTypeArray)): baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray, radii=baseRadiusArray, halfExtents=baseHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=meshScaleArray, collisionFramePositions=basePositionsArray, collisionFrameOrientations=baseOrientationsArray, physicsClientId=physicsClientId) urdfVisuals = base.urdf_visual_shapes baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals], halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals], radii=[v.geom_radius for v in urdfVisuals], lengths=[v.geom_length[0] for v in urdfVisuals], fileNames=[v.geom_meshfilename for v in urdfVisuals], meshScales=[v.geom_meshscale for v in urdfVisuals], rgbaColors=[v.material_rgba for v in urdfVisuals], visualFramePositions=[v.origin_xyz for v in urdfVisuals], visualFrameOrientations=[v.origin_rpy for v in urdfVisuals], physicsClientId=physicsClientId) # urdfVisual = base.urdf_visual_shapes[0] # baseVisualShapeIndex = p.createVisualShape(shapeType=urdfVisual.geom_type, # halfExtents=[ext * 0.5 for ext in urdfVisual.geom_extents], # radius=urdfVisual.geom_radius, # length=urdfVisual.geom_length[0], # fileName=urdfVisual.geom_meshfilename, # meshScale=urdfVisual.geom_meshscale, # rgbaColor=urdfVisual.material_rgba, # visualFramePosition=urdfVisual.origin_xyz, # visualFrameOrientation=urdfVisual.origin_rpy, # physicsClientId=physicsClientId) linkMasses=[] linkCollisionShapeIndices=[] linkVisualShapeIndices=[] linkPositions=[] linkOrientations=[] linkMeshScaleArray=[] linkInertialFramePositions=[] linkInertialFrameOrientations=[] linkParentIndices=[] linkJointTypes=[] linkJointAxis=[] for joint in self.urdfJoints: link = joint.link linkMass = link.urdf_inertial.mass linkCollisionShapeIndex=-1 linkVisualShapeIndex=-1 linkPosition=[0,0,0] linkOrientation=[0,0,0] linkInertialFramePosition=[0,0,0] linkInertialFrameOrientation=[0,0,0] linkParentIndex=self.linkNameToIndex[joint.parent_name] linkJointType=joint.joint_type linkJointAx = joint.joint_axis_xyz linkShapeTypeArray=[] linkRadiusArray=[] linkHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] linkPositionsArray=[] linkOrientationsArray=[] for v in link.urdf_collision_shapes: shapeType = v.geom_type linkShapeTypeArray.append(shapeType) linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]]) linkRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) linkMeshScaleArray.append(v.geom_meshscale) linkPositionsArray.append(v.origin_xyz) linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy)) if (len(linkShapeTypeArray)): linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray, radii=linkRadiusArray, halfExtents=linkHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=linkMeshScaleArray, collisionFramePositions=linkPositionsArray, collisionFrameOrientations=linkOrientationsArray, physicsClientId=physicsClientId) urdfVisuals = link.urdf_visual_shapes linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals], halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals], radii=[v.geom_radius for v in urdfVisuals], lengths=[v.geom_length[0] for v in urdfVisuals], fileNames=[v.geom_meshfilename for v in urdfVisuals], meshScales=[v.geom_meshscale for v in urdfVisuals], rgbaColors=[v.material_rgba for v in urdfVisuals], visualFramePositions=[v.origin_xyz for v in urdfVisuals], visualFrameOrientations=[v.origin_rpy for v in urdfVisuals], physicsClientId=physicsClientId) linkMasses.append(linkMass) linkCollisionShapeIndices.append(linkCollisionShapeIndex) linkVisualShapeIndices.append(linkVisualShapeIndex) linkPositions.append(joint.joint_origin_xyz) linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy)) linkInertialFramePositions.append(link.urdf_inertial.origin_xyz) linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy)) linkParentIndices.append(linkParentIndex) linkJointTypes.append(joint.joint_type) linkJointAxis.append(joint.joint_axis_xyz) obUid = p.createMultiBody(baseMass,\ baseCollisionShapeIndex=baseCollisionShapeIndex, baseVisualShapeIndex=baseVisualShapeIndex, basePosition=basePosition, baseInertialFramePosition=base.urdf_inertial.origin_xyz, baseInertialFrameOrientation=base.urdf_inertial.origin_rpy, linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=linkParentIndices, linkJointTypes=linkJointTypes, linkJointAxis=linkJointAxis, physicsClientId=physicsClientId) return obUid
def joinUrdf(self, childEditor, parentLinkIndex=0, jointPivotXYZInParent=[0, 0, 0], jointPivotRPYInParent=[0, 0, 0], jointPivotXYZInChild=[0, 0, 0], jointPivotRPYInChild=[0, 0, 0], parentPhysicsClientId=0, childPhysicsClientId=0): childLinkIndex = len(self.urdfLinks) insertJointIndex = len(self.urdfJoints) #combine all links, and add a joint for link in childEditor.urdfLinks: self.linkNameToIndex[link.link_name] = len(self.urdfLinks) self.urdfLinks.append(link) for joint in childEditor.urdfJoints: self.urdfJoints.append(joint) #add a new joint between a particular jointPivotQuatInChild = p.getQuaternionFromEuler(jointPivotRPYInChild) invJointPivotXYZInChild, invJointPivotQuatInChild = p.invertTransform( jointPivotXYZInChild, jointPivotQuatInChild) #apply this invJointPivot***InChild to all inertial, visual and collision element in the child link #inertial pos, orn = p.multiplyTransforms(self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz, p.getQuaternionFromEuler( self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy), invJointPivotXYZInChild, invJointPivotQuatInChild, physicsClientId=parentPhysicsClientId) self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p.getEulerFromQuaternion(orn) #all visual for v in self.urdfLinks[childLinkIndex].urdf_visual_shapes: pos, orn = p.multiplyTransforms(v.origin_xyz, p.getQuaternionFromEuler(v.origin_rpy), invJointPivotXYZInChild, invJointPivotQuatInChild) v.origin_xyz = pos v.origin_rpy = p.getEulerFromQuaternion(orn) #all collision for c in self.urdfLinks[childLinkIndex].urdf_collision_shapes: pos, orn = p.multiplyTransforms(c.origin_xyz, p.getQuaternionFromEuler(c.origin_rpy), invJointPivotXYZInChild, invJointPivotQuatInChild) c.origin_xyz = pos c.origin_rpy = p.getEulerFromQuaternion(orn) childLink = self.urdfLinks[childLinkIndex] parentLink = self.urdfLinks[parentLinkIndex] joint = UrdfJoint() joint.link = childLink joint.joint_name = "joint_dummy1" joint.joint_type = p.JOINT_REVOLUTE joint.joint_lower_limit = 0 joint.joint_upper_limit = -1 joint.parent_name = parentLink.link_name joint.child_name = childLink.link_name joint.joint_origin_xyz = jointPivotXYZInParent joint.joint_origin_rpy = jointPivotRPYInParent joint.joint_axis_xyz = [0, 0, 1] #the following commented line would crash PyBullet, it messes up the joint indexing/ordering #self.urdfJoints.append(joint) #so make sure to insert the joint in the right place, to links/joints match self.urdfJoints.insert(insertJointIndex, joint) return joint
def main(): pygame.init() screen = pygame.display.set_mode((800, 480), pygame.OPENGL | pygame.DOUBLEBUF) glEnable(GL_DEPTH_TEST) shaderProgram = shaders.compileProgram( shaders.compileShader(vertexShader, GL_VERTEX_SHADER), shaders.compileShader(fragmentShader, GL_FRAGMENT_SHADER)) boxPos = [0.0, 0.0, 5.0] boxOrn = p.getQuaternionFromEuler([0.0, 0.0, 0.0]) planePos = [0.0, 0.0, 0.0] planeOrn = p.getQuaternionFromEuler([0.0, 0.0, 0.0]) physicsClientId = initPyBullet() objectManager = URDFManager(physicsClientId) planeId = objectManager.add("data/plane.urdf", planePos, planeOrn) boxId = objectManager.add("data/007-box.urdf", boxPos, boxOrn) shapes = objectManager.getVisualShapes(planeId) for key, visualShape in shapes.items(): visualShape.vs.prepare(shaderProgram) shapes = objectManager.getVisualShapes(boxId) for key, visualShape in shapes.items(): visualShape.vs.prepare(shaderProgram) print("----------") print(p.getNumJoints(boxId)) print(p.getJointInfo(boxId, 0)) """ for i in range(p.getNumJoints(r2d2)): jointInfo = p.getJointInfo(r2d2, i) print("-------"+str(i)+"-------") print(jointInfo) """ #GEOM_SPHERE, GEOM_BOX, GEOM_CAPSULE, GEOM_CYLINDER, GEOM_PLANE, GEOM_MESH """ visualShapes = p.getVisualShapeData(r2d2) i = 0 for vs in visualShapes: print("-------"+str(i)+"-------") print(vs) i += 1 """ clock = pygame.time.Clock() done = False tick = 0 velocity = 0.0 force = 0.0 while not done: for event in pygame.event.get(): if event.type == pygame.QUIT: done = True elif event.type == pygame.KEYDOWN: if event.key == pygame.K_UP: velocity = -100 force = 250.0 elif event.key == pygame.K_DOWN: velocity = 100 force = 250.0 elif event.type == pygame.KEYUP: if event.key == pygame.K_UP or event.key == pygame.K_DOWN: velocity = 0.0 force = 0.0 p.setJointMotorControl2(bodyUniqueId=boxId, jointIndex=0, controlMode=p.VELOCITY_CONTROL, targetVelocity=velocity, force=force) tick += 1 startDisplay() shapes = objectManager.getVisualShapes(planeId) for key, visualShape in shapes.items(): #visualShape.updatePosAndOrn(planePos, planeOrn) visualShape.vs.draw(shaderProgram) quatBoxPos, quatBoxOrn = p.getBasePositionAndOrientation(boxId) boxPos = TranslationMatrix(quatBoxPos) boxOrn = getOrnMatrixFromQuaternion(quatBoxOrn) shapes = objectManager.getVisualShapes(boxId) for key, visualShape in shapes.items(): if (key != -1): linkState = p.getLinkState(boxId, key) quatLinkPos = linkState[0] quatLinkOrn = linkState[1] linkPos = TranslationMatrix(quatLinkPos) linkOrn = getOrnMatrixFromQuaternion(quatLinkOrn) else: linkPos = boxPos linkOrn = boxOrn pos = linkPos orn = visualShape.orn @ linkOrn visualShape.vs.updatePosAndOrn(pos, orn) visualShape.vs.draw(shaderProgram) endDisplay() p.stepSimulation() pygame.display.flip() clock.tick(60)
def createMultiBody(self, basePosition=[0, 0, 0], baseOrientation=[0, 0, 0, 1], physicsClientId=0): #assume link[0] is base if (len(self.urdfLinks) == 0): return -1 #for i in range (len(self.urdfLinks)): # print("link", i, "=",self.urdfLinks[i].link_name) base = self.urdfLinks[0] #v.tmp_collision_shape_ids=[] baseMass = base.urdf_inertial.mass baseCollisionShapeIndex = -1 baseShapeTypeArray = [] baseRadiusArray = [] baseHalfExtentsArray = [] lengthsArray = [] fileNameArray = [] meshScaleArray = [] basePositionsArray = [] baseOrientationsArray = [] for v in base.urdf_collision_shapes: shapeType = v.geom_type baseShapeTypeArray.append(shapeType) baseHalfExtentsArray.append( [0.5 * v.geom_extents[0], 0.5 * v.geom_extents[1], 0.5 * v.geom_extents[2]]) baseRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) meshScaleArray.append(v.geom_meshscale) basePositionsArray.append(v.origin_xyz) orn = p.getQuaternionFromEuler(v.origin_rpy) baseOrientationsArray.append(orn) if (len(baseShapeTypeArray)): #print("fileNameArray=",fileNameArray) baseCollisionShapeIndex = p.createCollisionShapeArray( shapeTypes=baseShapeTypeArray, radii=baseRadiusArray, halfExtents=baseHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=meshScaleArray, collisionFramePositions=basePositionsArray, collisionFrameOrientations=baseOrientationsArray, physicsClientId=physicsClientId) urdfVisuals = base.urdf_visual_shapes shapeTypes = [v.geom_type for v in urdfVisuals] halfExtents = [[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals] radii = [v.geom_radius for v in urdfVisuals] lengths = [v.geom_length for v in urdfVisuals] fileNames = [v.geom_meshfilename for v in urdfVisuals] meshScales = [v.geom_meshscale for v in urdfVisuals] rgbaColors = [v.material_rgba for v in urdfVisuals] visualFramePositions = [v.origin_xyz for v in urdfVisuals] visualFrameOrientations = [p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals] baseVisualShapeIndex = -1 if (len(shapeTypes)): #print("len(shapeTypes)=",len(shapeTypes)) #print("len(halfExtents)=",len(halfExtents)) #print("len(radii)=",len(radii)) #print("len(lengths)=",len(lengths)) #print("len(fileNames)=",len(fileNames)) #print("len(meshScales)=",len(meshScales)) #print("len(rgbaColors)=",len(rgbaColors)) #print("len(visualFramePositions)=",len(visualFramePositions)) #print("len(visualFrameOrientations)=",len(visualFrameOrientations)) baseVisualShapeIndex = p.createVisualShapeArray( shapeTypes=shapeTypes, halfExtents=halfExtents, radii=radii, lengths=lengths, fileNames=fileNames, meshScales=meshScales, rgbaColors=rgbaColors, visualFramePositions=visualFramePositions, visualFrameOrientations=visualFrameOrientations, physicsClientId=physicsClientId) linkMasses = [] linkCollisionShapeIndices = [] linkVisualShapeIndices = [] linkPositions = [] linkOrientations = [] linkInertialFramePositions = [] linkInertialFrameOrientations = [] linkParentIndices = [] linkJointTypes = [] linkJointAxis = [] for joint in self.urdfJoints: link = joint.link linkMass = link.urdf_inertial.mass linkCollisionShapeIndex = -1 linkVisualShapeIndex = -1 linkPosition = [0, 0, 0] linkOrientation = [0, 0, 0] linkInertialFramePosition = [0, 0, 0] linkInertialFrameOrientation = [0, 0, 0] linkParentIndex = self.linkNameToIndex[joint.parent_name] linkJointType = joint.joint_type linkJointAx = joint.joint_axis_xyz linkShapeTypeArray = [] linkRadiusArray = [] linkHalfExtentsArray = [] lengthsArray = [] fileNameArray = [] linkMeshScaleArray = [] linkPositionsArray = [] linkOrientationsArray = [] for v in link.urdf_collision_shapes: shapeType = v.geom_type linkShapeTypeArray.append(shapeType) linkHalfExtentsArray.append( [0.5 * v.geom_extents[0], 0.5 * v.geom_extents[1], 0.5 * v.geom_extents[2]]) linkRadiusArray.append(v.geom_radius) lengthsArray.append(v.geom_length) fileNameArray.append(v.geom_meshfilename) linkMeshScaleArray.append(v.geom_meshscale) linkPositionsArray.append(v.origin_xyz) linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy)) if (len(linkShapeTypeArray)): linkCollisionShapeIndex = p.createCollisionShapeArray( shapeTypes=linkShapeTypeArray, radii=linkRadiusArray, halfExtents=linkHalfExtentsArray, lengths=lengthsArray, fileNames=fileNameArray, meshScales=linkMeshScaleArray, collisionFramePositions=linkPositionsArray, collisionFrameOrientations=linkOrientationsArray, physicsClientId=physicsClientId) urdfVisuals = link.urdf_visual_shapes linkVisualShapeIndex = -1 shapeTypes = [v.geom_type for v in urdfVisuals] halfExtents = [[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals] radii = [v.geom_radius for v in urdfVisuals] lengths = [v.geom_length for v in urdfVisuals] fileNames = [v.geom_meshfilename for v in urdfVisuals] meshScales = [v.geom_meshscale for v in urdfVisuals] rgbaColors = [v.material_rgba for v in urdfVisuals] visualFramePositions = [v.origin_xyz for v in urdfVisuals] visualFrameOrientations = [p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals] if (len(shapeTypes)): print("fileNames=", fileNames) linkVisualShapeIndex = p.createVisualShapeArray( shapeTypes=shapeTypes, halfExtents=halfExtents, radii=radii, lengths=lengths, fileNames=fileNames, meshScales=meshScales, rgbaColors=rgbaColors, visualFramePositions=visualFramePositions, visualFrameOrientations=visualFrameOrientations, physicsClientId=physicsClientId) linkMasses.append(linkMass) linkCollisionShapeIndices.append(linkCollisionShapeIndex) linkVisualShapeIndices.append(linkVisualShapeIndex) linkPositions.append(joint.joint_origin_xyz) linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy)) linkInertialFramePositions.append(link.urdf_inertial.origin_xyz) linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy)) linkParentIndices.append(linkParentIndex) linkJointTypes.append(joint.joint_type) linkJointAxis.append(joint.joint_axis_xyz) obUid = p.createMultiBody(baseMass,\ baseCollisionShapeIndex=baseCollisionShapeIndex, baseVisualShapeIndex=baseVisualShapeIndex, basePosition=basePosition, baseOrientation=baseOrientation, baseInertialFramePosition=base.urdf_inertial.origin_xyz, baseInertialFrameOrientation=p.getQuaternionFromEuler(base.urdf_inertial.origin_rpy), linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=linkParentIndices, linkJointTypes=linkJointTypes, linkJointAxis=linkJointAxis, physicsClientId=physicsClientId) return obUid
amplitude = 0.8 jump_amp = 0.5 maxForce = 3.5 kneeFrictionForce = 0 kp = 1 kd = .5 maxKneeForce = 1000 # physId = p.connect(p.SHARED_MEMORY_GUI) if (1): p.connect(p.GUI) p.resetSimulation() p.setAdditionalSearchPath(pybullet_data.getDataPath()) angle = 0 # pick in range 0..0.2 radians orn = p.getQuaternionFromEuler([0, angle, 0]) p.loadURDF("plane.urdf", [0, 0, 0], orn) p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) # p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, # "genericlogdata.bin", # maxLogDof=16, # logFlags=p.STATE_LOG_JOINT_TORQUES) p.setTimeOut(4000000) p.setGravity(0, 0, 0) p.setTimeStep(fixedTimeStep) orn = p.getQuaternionFromEuler([0, 0, 0.4]) p.setRealTimeSimulation(0) quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3], orn,
def __init__(self, gui, urdf_path=None, foot_link_names=[], terrain_height=0, field=False, joints_ft=False, robot="wolfgang", load_robot=True): self.gui = gui self.paused = False self.gravity = True self.terrain_height = terrain_height self.field_on = field self.urdf_path = urdf_path self.foot_link_names = foot_link_names self.joints_ft = joints_ft self.robot_type = robot self.load_robot = load_robot self.robot_indexes = [] self.joints = {} self.joint_stall_torques = {} self.joint_max_vels = {} self.link_masses = {} self.link_inertias = {} self.pressure_sensors = {} self.links = {} self.torso_ids = {} self.last_step_time = 0 self.realtime = False self.time_multiplier = 0 # config values self.start_position = [0, 0, 0.43] self.start_orientation = p.getQuaternionFromEuler((0, 0.25, 0)) if self.robot_type is None: # no robot to initialize pass elif self.robot_type == "wolfgang": self.initial_joint_positions = { "LAnklePitch": -30, "LAnkleRoll": 0, "LHipPitch": 30, "LHipRoll": 0, "LHipYaw": 0, "LKnee": 60, "RAnklePitch": 30, "RAnkleRoll": 0, "RHipPitch": -30, "RHipRoll": 0, "RHipYaw": 0, "RKnee": -60, "LShoulderPitch": 75, "LShoulderRoll": 0, "LElbow": 36, "RShoulderPitch": -75, "RShoulderRoll": 0, "RElbow": -36, "HeadPan": 0, "HeadTilt": 0 } elif self.robot_type in ["op2", "robotis_op2"]: self.initial_joint_positions = { "l_ankle_pitch": 0, "l_ankle_roll": 0, "l_hip_pitch": 0, "l_hip_roll": 0, "l_hip_yaw": 0, "l_knee": 0, "r_ankle_pitch": 0, "r_ankle_roll": 0, "r_hip_pitch": 0, "r_hip_roll": 0, "r_hip_yaw": 0, "r_knee": 0, "l_sho_pitch": 0, "l_sho_roll": 0, "LElbow": 0, "r_sho_pitch": 0, "r_sho_roll": 0, "RElbow": 0, "head_pan": 0, "head_tilt": 0 } self.foot_link_names = ["l_foot", "r_foot"] elif self.robot_type == "sigmaban": self.start_orientation = p.getQuaternionFromEuler((0, 0.0, 0)) self.initial_joint_positions = { "left_ankle_pitch": 0, "left_ankle_roll": 0, "left_hip_pitch": 0, "left_hip_roll": 0, "left_hip_yaw": 0, "left_knee": 0, "right_ankle_pitch": 0, "right_ankle_roll": 0, "right_hip_pitch": 0, "right_hip_roll": 0, "right_hip_yaw": 0, "right_knee": 0, "left_shoulder_pitch": 0, "left_shoulder_roll": 0, "LElbow": 0, "right_shoulder_pitch": 0, "right_shoulder_roll": 0, "RElbow": 0, "head_yaw": 0, "head_pitch": 0 } else: print(f"robot {self.robot_type} not known") quit(0) # Instantiating Bullet if self.gui: self.client_id = p.connect(p.GUI) else: self.client_id = p.connect(p.DIRECT) if self.gravity: p.setGravity(0, 0, -9.81) self.time = 0 # disable debug interface, only show robot p.configureDebugVisualizer(p.COV_ENABLE_GUI, False) # Engine parameters # time step should be at 240Hz (due to pyBullet documentation) self.timestep = 1 / 240 # standard parameters seem to be best. leave them like they are # p.setPhysicsEngineParameter(fixedTimeStep=self.timestep, numSubSteps=1) # no real time, as we will publish own clock, but we have option to switch to real_time by waiting p.setRealTimeSimulation(0) self.last_wall_time = time() self.load_models()
import pybullet as p import time cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True) rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0) pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0) yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0) fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0) fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0) fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0) while True: roll = p.readUserDebugParameter(rollId) pitch = p.readUserDebugParameter(pitchId) yaw = p.readUserDebugParameter(yawId) x = p.readUserDebugParameter(fwdxId) y = p.readUserDebugParameter(fwdyId) z = p.readUserDebugParameter(fwdzId) orn = p.getQuaternionFromEuler([roll, pitch, yaw]) p.resetBasePositionAndOrientation(q, [x, y, z], orn) #p.stepSimulation()#not really necessary for this demo, no physics used
import pickle as pk import numpy as np import sys sys.path.append('../../envs') import pybullet as pb from humanoid import PoppyHumanoidEnv, convert_angles env = PoppyHumanoidEnv(pb.POSITION_CONTROL) N = len(env.joint_index) with open("../../../scripts/stand.pkl", "rb") as f: stand_dict = pk.load(f) stand = env.angle_array(stand_dict) env.set_position(stand) pb.resetBasePositionAndOrientation(env.robot_id, (0, .5, 1), pb.getQuaternionFromEuler((0, 0, 0))) input('.')
speed = 10 amplitude = 0.8 jump_amp = 0.5 maxForce = 3.5 kneeFrictionForce = 0 kp = 1 kd = .5 maxKneeForce = 1000 physId = p.connect(p.SHARED_MEMORY) if (physId < 0): p.connect(p.GUI) #p.resetSimulation() angle = 0 # pick in range 0..0.2 radians orn = p.getQuaternionFromEuler([0, angle, 0]) p.loadURDF("plane.urdf", [0, 0, 0], orn) p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof=16, logFlags=p.STATE_LOG_JOINT_TORQUES) p.setTimeOut(4000000) p.setGravity(0, 0, 0) p.setTimeStep(fixedTimeStep) orn = p.getQuaternionFromEuler([0, 0, 0.4]) p.setRealTimeSimulation(0) quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3], orn,
renderer=p.ER_BULLET_HARDWARE_OPENGL) return images if __name__ == "__main__": clid = p.connect(p.SHARED_MEMORY) if (clid < 0): p.connect(p.GUI) #p.connect(p.SHARED_MEMORY_GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("checkerboard/calibration.urdf", [0.5, 0, 0.01]) p.loadURDF("plane.urdf", [0, 0, 0.0]) d435Id = p.loadURDF("d435/d435.urdf", [0, 0, 0.0]) p.resetBasePositionAndOrientation( d435Id, [1, 0, 1], p.getQuaternionFromEuler([0, -math.pi / 4, 0])) pandaId = p.loadURDF("Panda/panda.urdf", [0, 0, 0]) p.resetBasePositionAndOrientation(pandaId, [0, 0, 0], [0, 0, 0, 1]) kukaEndEffectorIndex = 6 numJoints = 7 rospy.init_node('listener', anonymous=True) rospy.Subscriber("/joint_states_desired", JointState, callback) pub = rospy.Publisher("point_cloud2", PointCloud2, queue_size=2) count = 0 while 1: #print("joint state : ",joint_states) d435pos, d435orn = p.getBasePositionAndOrientation(d435Id) d435orn = p.getEulerFromQuaternion(d435orn) for i in range(numJoints): p.resetJointState(pandaId, i, joint_states[i])
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0]) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1]) kukaEndEffectorIndex = 6 numJoints = p.getNumJoints(kukaId) #Joint damping coefficents. Using large values for the joints that we don't want to move. jd = [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 0.5] #jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5] p.setGravity(0, 0, 0) while 1: p.stepSimulation() for i in range(1): pos = [0, 0, 1.26] orn = p.getQuaternionFromEuler([0, 0, 3.14]) jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, orn, jointDamping=jd) for i in range(numJoints): p.setJointMotorControl2(bodyIndex=kukaId, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=jointPoses[i], targetVelocity=0, force=500, positionGain=0.03,
def estimate_pose(self, start_pos, z_distance=2, start_orientation=(0, 0, 0)): start_time = datetime.datetime.now() start_quat_orientation = p.getQuaternionFromEuler(start_orientation) start_pos = [start_pos[0], start_pos[1], start_pos[2] + z_distance] boxId = p.loadURDF(self.urdf_path, start_pos, start_quat_orientation) linkId = 1 jointFrictionForce = 0 for joint in range(p.getNumJoints(boxId)): p.setJointMotorControl2(boxId, joint, p.POSITION_CONTROL, force=jointFrictionForce) linkStateFull = p.getLinkState(boxId, linkId) prevCubePos, prevCubeOrn = linkStateFull[4], linkStateFull[5] for i in range(1000): p.stepSimulation() linkStateFull = p.getLinkState(boxId, linkId) cubePos, cubeOrn = linkStateFull[4], linkStateFull[5] dist = math.sqrt((cubePos[2] - prevCubePos[2])**2) # only z q0 = pyquaternion.Quaternion(x=cubeOrn[0], y=cubeOrn[1], z=cubeOrn[2], w=cubeOrn[3]) q1 = pyquaternion.Quaternion(x=prevCubeOrn[0], y=prevCubeOrn[1], z=prevCubeOrn[2], w=prevCubeOrn[3]) quat_dist = pyquaternion.Quaternion.absolute_distance(q0, q1) # print "dist:{:.8f}".format(dist), "quat_dist:{:.6f}".format(quat_dist) prevCubePos, prevCubeOrn = cubePos, cubeOrn if i > 10 and (dist <= 0.00001 and quat_dist <= 0.0001): #print "breaking at step:", i, dist, quat_dist break # time.sleep(1./360.) end_time = datetime.datetime.now() delta = end_time - start_time delta_millis = int(delta.total_seconds() * 1000) # milliseconds #print "total time millis:{}".format(delta_millis) p.removeBody(boxId) euler_q1 = p.getEulerFromQuaternion(prevCubeOrn) R = mesh_helper.Rxyz(euler_q1[0], euler_q1[1], euler_q1[2]) zf = np.array([0, 0, 1]) zf_l = np.dot(R, zf) return prevCubePos, zf_l
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH, fileName="marble_cube.obj", rgbaColor=[1, 1, 1, 1], specularColor=[1, 1, 1], visualFramePosition=shift, meshScale=meshScale) #collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="textures/marble_cube.obj", collisionFramePosition=shift,meshScale=meshScale) collisionShapeId = -1 uiCube = p.createMultiBody(baseMass=0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[0, 1, 0], useMaximalCoordinates=True) textOrn = p.getQuaternionFromEuler([0, 0, -1.5707963]) numLines = 1 lines = [-1] * numLines p.stepSimulation() #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)] kitchenObj = p.loadSDF("kitchens/1.sdf") #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=")
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally p.resetDebugVisualizerCamera(cameraDistance=0.45, cameraYaw=135, cameraPitch=-45, cameraTargetPosition=[0, 0, 0]) p.setGravity(0, 0, -10) frequency = 100 # Hz p.setTimeStep(1 / frequency) p.setRealTimeSimulation(0) planeId = p.loadURDF(URDF(get_scene("plane-big.urdf.xml")).get_path()) startPos = [0, 0, 0] # RGB = xyz startOrientation = p.getQuaternionFromEuler( [0, 0, 0]) # rotated around which axis? # np.deg2rad(90) # rotating a standing cylinder around the y axis, puts it flat onto the x axis robot = p.loadURDF("urdfs/PoppyErgoJr_pen.urdf.xml", startPos, startOrientation, useFixedBase=1) for i in range(p.getNumJoints(robot)): print(p.getJointInfo(robot, i)) motors = [0, 1, 2, 3, 4, 5] # debugParams = [] for i in range(len(motors)):
#first try to connect to shared memory (VR), if it fails use local GUI c = p.connect(p.SHARED_MEMORY) if (c < 0): c = p.connect(p.GUI) #p.resetSimulation() p.setGravity(0, 0, -10) print(c) if (c < 0): p.connect(p.GUI) #load the MuJoCo MJCF hand objects = p.loadMJCF("MPL/mpl2.xml") hand = objects[0] ho = p.getQuaternionFromEuler([0, 3.14, 0]) hand_cid = p.createConstraint(hand, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.1, 0, 0], [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 POSITION = 1 ORIENTATION = 2 BUTTONS = 6
# plane est le sol il est contenu dans l'import pybullet_data pybuplane = pybullet.loadURDF("plane.urdf") #os.system("git clone https://github.com/Chris-Annin/ROS_AR2_urdf.git ar2") #pybullet.setGravity(0,0,-9.8) # on récpère le bras robot depuiq le dossier git qu'on a récupéré # c'est important de lui mettre une useFixedBase à 1 pour qu'il ne tombe pas (avec la gravité par exemple) robot = pybullet.loadURDF("ar2/urdf/ar2.urdf", [0, 0, 0], useFixedBase=1) # importation du socle socle = pybullet.loadURDF("socle.urdf", [0, -0.5, 0], useFixedBase=1) # pour faire l'orientation du cub orientation = pybullet.getQuaternionFromEuler([73, 0, 0]) # importation du cube cube = pybullet.loadURDF("cube.urdf", [0.047, -0.46, 0.07], orientation) pybullet.resetDebugVisualizerCamera(1.40, -53.0, -39.0, (0.53, 0.21, -0.24)) numJoints = pybullet.getNumJoints(robot) time.sleep(2) pybullet.setTimeStep(0.0001) pybullet.setRealTimeSimulation(0) steps = 400 fixedTimeStep = 1.5 / steps robotEndEffectorIndex = numJoints - 1 pybullet.setGravity(0, 0, 0) t = 0.
import pybullet as p import time p.connect(p.GUI) useCollisionShapeQuery = True p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1) geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.2, 0.2]) baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0]) #[0,0.5,0.5,0] basePositionB = [1.5, 0, 1] obA = -1 obB = -1 obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1]) obB = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geomBox, basePosition=basePositionB, baseOrientation=baseOrientationB) lineWidth = 3 colorRGB = [1, 0, 0] lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0], lineToXYZ=[0, 0, 0], lineColorRGB=colorRGB, lineWidth=lineWidth, lifeTime=0) pitch = 0 yaw = 0 while (p.isConnected()): pitch += 0.01 if (pitch >= 3.1415 * 2.):
def init(self): if (self._game_settings['render']): # self._object.setPosition([self._x[self._step], self._y[self._step], 0.0] ) self._physicsClient = p.connect(p.GUI) else: self._physicsClient = p.connect(p.DIRECT) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation() #p.setRealTimeSimulation(True) p.setGravity(0,0,self._GRAVITY) p.setTimeStep(self._dt) planeId = p.loadURDF("plane.urdf") cubeStartPos = [0,0,0.5] cubeStartOrientation = p.getQuaternionFromEuler([0.,0,0]) self._agent = p.loadURDF("sphere2.urdf", cubeStartPos, cubeStartOrientation) self._blocks = [] self._num_blocks=0 for i in range(self._num_blocks): blockId = p.loadURDF("cube2.urdf", [2.0,2.0,0.5], cubeStartOrientation, useFixedBase=1) self._blocks.append(blockId) self._target = p.loadURDF("sphere2red.urdf", cubeStartPos, cubeStartOrientation, useFixedBase=1) #disable the default velocity motors #and set some position control with small force to emulate joint friction/return to a rest pose jointFrictionForce=1 for joint in range (p.getNumJoints(self._agent)): p.setJointMotorControl2(self._agent,joint,p.POSITION_CONTROL,force=jointFrictionForce) #for i in range(10000): # p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0) # p.stepSimulation() #import ipdb #ipdb.set_trace() p.setRealTimeSimulation(1) lo = [0.0 for l in self.getObservation()[0]] hi = [1.0 for l in self.getObservation()[0]] state_bounds_llc = [lo, hi] lo = [0.0 for l in self.getObservation()[1]] hi = [1.0 for l in self.getObservation()[1]] state_bounds_hlc = [lo, hi] state_bounds = [state_bounds_llc, state_bounds_hlc] print ("NavGameHRL2D state bounds: ", state_bounds) self._game_settings['state_bounds'] = [lo, hi] self._state_length = len(self._game_settings['state_bounds'][0]) print ("self._state_length: ", self._state_length) self._observation_space = ActionSpace(self._game_settings['state_bounds']) self._last_state = self.getState() self._last_pose = p.getBasePositionAndOrientation(self._agent)[0]
kd = .1 physId = p.connect(p.SHARED_MEMORY) if (physId<0): p.connect(p.GUI) #p.resetSimulation() p.loadURDF("plane.urdf",0,0,0) p.setPhysicsEngineParameter(numSolverIterations=50) p.setTimeOut(4) p.setGravity(0,0,0) p.setTimeStep(fixedTimeStep) orn = p.getQuaternionFromEuler([0,0,0.4]) p.setRealTimeSimulation(0) quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,0,0.2],orn,useFixedBase=False) nJoints = p.getNumJoints(quadruped) jointNameToId = {} for i in range(nJoints): jointInfo = p.getJointInfo(quadruped, i) jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] hip_front_rightR_link = jointNameToId['hip_front_rightR_link'] knee_front_rightR_link = jointNameToId['knee_front_rightR_link'] motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint'] motor_front_rightL_link = jointNameToId['motor_front_rightL_link']
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
i = 4 p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=e[ANALOG] * 0.05, force=10) i = 6 p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=e[ANALOG] * 0.05, force=10) if sq_len < THRESHOLD * THRESHOLD: eef_pos = e[POSITION] eef_orn = p.getQuaternionFromEuler([0, -math.pi, 0]) joint_pos = p.calculateInverseKinematics(kuka, 6, eef_pos, eef_orn, lowerLimits=LOWER_LIMITS, upperLimits=UPPER_LIMITS, jointRanges=JOINT_RANGE, restPoses=REST_POSE, jointDamping=JOINT_DAMP) for i in range(len(joint_pos)): p.setJointMotorControl2(kuka, i, p.POSITION_CONTROL, targetPosition=joint_pos[i],
controlMode=p.VELOCITY_CONTROL, targetVelocity=0, force=standstilltorque) p.setJointMotorControl2(bodyIndex=robot, jointIndex=spring_motor_id, controlMode=p.VELOCITY_CONTROL, targetVelocity=0, force=standstilltorque) physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally p.setGravity(0, 0, -10) planeId = p.loadURDF("plane.urdf") robotStartPos = [0, 0, 0.23] robotStartOrientation = p.getQuaternionFromEuler([np.pi / 2, 0, 0]) robot = p.loadURDF("/home/pramod/Single_leg_test/Urdf/Test_1/urdf/Test_1.urdf", robotStartPos, robotStartOrientation, useFixedBase=1) # ResetLeg() a = 0.06 b = 0.03 x_path = [] z_path = [] y_path = [] for j in range(0, 361, 5): # Ellipse x = -0.06 + a * m.cos(m.radians(j)) z = 0.035 + b * m.sin(m.radians(j))
p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json") jran = 50 iran = 100 num=64 radius=0.1 numDominoes=0 for i in range (int(num*50)): num=(radius*2*math.pi)/0.08 radius += 0.05/float(num) orn = p.getQuaternionFromEuler([0,0,0.5*math.pi+math.pi*2*i/float(num)]) pos = [radius*math.cos(2*math.pi*(i/float(num))),radius*math.sin(2*math.pi*(i/float(num))), 0.03] sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates) numDominoes+=1 pos=[pos[0],pos[1],pos[2]+0.3] orn = p.getQuaternionFromEuler([0,0,-math.pi/4.]) sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates) print("numDominoes=",numDominoes) #for j in range (20): # for i in range (100): # if (i<99): # sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
def init_grasp(self): try: p.removeBody(self.box_id) except: pass pos_traj = np.load(os.path.join(self.env_root, 'init', 'pos.npy')) orn_traj = np.load(os.path.join(self.env_root, 'init', 'orn.npy')) self.fix_orn = np.load(os.path.join(self.env_root, 'init', 'orn.npy')) for j in range(7): self.p.resetJointState(self.robotId, j, self.data_q[0][j], self.data_dq[0][j]) self.robot.gripperControl(0) for init_t in range(100): box = p.getAABB(self.obj_id, -1) center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])] center[0] -= 0.05 center[1] -= 0.05 center[2] += 0.03 # center = (box[0]+box[1])*0.5 points = np.array([pos_traj[0], center]) start_id = 0 init_traj = point2traj(points) start_id = self.move(init_traj, orn_traj, start_id) # grasping grasp_stage_num = 10 for grasp_t in range(grasp_stage_num): gripperPos = grasp_t / float(grasp_stage_num) * 180.0 self.robot.gripperControl(gripperPos) start_id += 1 pos = p.getLinkState(self.robotId, 7)[0] up_traj = point2traj([pos, [pos[0], pos[1] + 0.05, pos[2] + 0.3]]) start_id = self.move(up_traj, orn_traj, start_id) if self.opt.rand_start == 'rand': # move in z-axis direction pos = p.getLinkState(self.robotId, 7)[0] up_traj = point2traj([ pos, [pos[0], pos[1], pos[2] + (random.random() - 0.5) * 0.1] ]) start_id = self.move(up_traj, orn_traj, start_id) # move in y-axis direction pos = p.getLinkState(self.robotId, 7)[0] up_traj = point2traj([ pos, [pos[0], pos[1] + (random.random() - 0.5) * 0.2 + 0.2, pos[2]] ]) start_id = self.move(up_traj, orn_traj, start_id) # move in x-axis direction pos = p.getLinkState(self.robotId, 7)[0] up_traj = point2traj([ pos, [pos[0] + (random.random() - 0.5) * 0.2, pos[1], pos[2]] ]) start_id = self.move(up_traj, orn_traj, start_id) elif self.opt.rand_start == 'two': prob = random.random() if prob < 0.5: pos = p.getLinkState(self.robotId, 7)[0] up_traj = point2traj([pos, [pos[0], pos[1] + 0.2, pos[2]]]) start_id = self.move(up_traj, orn_traj, start_id) if self.opt.rand_box == 'rand': self.box_file = os.path.join(self.env_root, "urdf/openbox/openbox.urdf") self.box_position = [ 0.42 + (random.random() - 0.5) * 0.2, 0.00 + (random.random() - 0.5) * 0.4, 0.34 ] self.box_scaling = 0.00037 self.box_orientation = p.getQuaternionFromEuler( [0, 0, math.pi / 2]) self.box_id = p.loadURDF(fileName=self.box_file, basePosition=self.box_position, baseOrientation=self.box_orientation, globalScaling=self.box_scaling ) #, physicsClientId=self.physical_id) else: self.box_file = os.path.join(self.env_root, "urdf/openbox/openbox.urdf") self.box_position = [0.42, 0.00, 0.34] self.box_scaling = 0.00037 self.box_orientation = p.getQuaternionFromEuler( [0, 0, math.pi / 2]) self.box_id = p.loadURDF(fileName=self.box_file, basePosition=self.box_position, baseOrientation=self.box_orientation, globalScaling=self.box_scaling ) #, physicsClientId=self.physical_id) texture_path = os.path.join(self.env_root, 'texture/sun_textures') texture_file = os.path.join( texture_path, random.sample(os.listdir(texture_path), 1)[0]) textid = p.loadTexture(texture_file) # p.changeVisualShape (self.box_id, -1, rgbaColor=[1, 1, 1, 0.9]) # p.changeVisualShape (self.box_id, -1, textureUniqueId=textid) p.changeVisualShape(self.box_id, -1, rgbaColor=[1, 0, 0, 1]) self.start_pos = p.getLinkState(self.robotId, 7)[0] box = p.getAABB(self.box_id, -1) box_center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])] obj = p.getAABB(self.obj_id, -1) obj_center = [(x + y) * 0.5 for x, y in zip(obj[0], obj[1])] self.last_aabb_dist = sum([ (x - y)**2 for x, y in zip(box_center, obj_center) ])**0.5
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])
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])
for i in range(segmentLength): p.createMultiBody(baseMass=0, baseCollisionShapeIndex=colBoxId, basePosition=[segmentStart, 0, -0.1]) segmentStart = segmentStart - 1 for i in range(segmentLength): height = 0 if (i % 2): height = 1 p.createMultiBody(baseMass=0, baseCollisionShapeIndex=colBoxId, basePosition=[segmentStart, 0, -0.1 + height]) segmentStart = segmentStart - 1 baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.]) for i in range(segmentLength): p.createMultiBody(baseMass=0, baseCollisionShapeIndex=colBoxId, basePosition=[segmentStart, 0, -0.1]) segmentStart = segmentStart - 1 if (i % 2): p.createMultiBody(baseMass=0, baseCollisionShapeIndex=colBoxId, basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth], baseOrientation=baseOrientation) for i in range(segmentLength): p.createMultiBody(baseMass=0, baseCollisionShapeIndex=colBoxId,
import pybullet as p import pybullet_data import time physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF p.setGravity(0,0,-10) planeId = p.loadURDF("road_plain.urdf") cubeStartPos = [0,0,1] cubeStartOrientation = p.getQuaternionFromEuler([0,0,0]) boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) p.stepSimulation() cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) print(cubePos,cubeOrn) time.sleep(5) p.disconnect()
import pybullet as p from time import sleep import matplotlib.pyplot as plt import numpy as np physicsClient = p.connect(p.GUI) p.setGravity(0,0,0) bearStartPos1 = [-3.3,0,0] bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0]) bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1) bearStartPos2 = [0,0,0] bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0]) bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2) textureId = p.loadTexture("checker_grid.jpg") #p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId) #p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId) useRealTimeSimulation = 1 if (useRealTimeSimulation): p.setRealTimeSimulation(1) while 1: if (useRealTimeSimulation): camera = p.getDebugVisualizerCamera() viewMat = camera[2] projMat = camera[3] #An example of setting the view matrix for the projective texture. #viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
#coord = p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True) p.setGravity(0, 0, -10) p.setRealTimeSimulation(0) #coordPos = [0,0,0] #coordOrnEuler = [0,0,0] #coordPos= [0.7000000000000004, 0, 0.22000000000000006] #coordOrnEuler= [0, -0.2617993877991496, 0] coordPos = [0.07, 0, -0.6900000000000004] coordOrnEuler = [0, 0, 0] coordPos2 = [0, 0, 0] coordOrnEuler2 = [0, 0, 0] invPos, invOrn = p.invertTransform(coordPos, p.getQuaternionFromEuler(coordOrnEuler)) mPos, mOrn = p.multiplyTransforms(invPos, invOrn, coordPos2, p.getQuaternionFromEuler(coordOrnEuler2)) eul = p.getEulerFromQuaternion(mOrn) print("rpy=\"", eul[0], eul[1], eul[2], "\" xyz=\"", mPos[0], mPos[1], mPos[2]) shift = 0 gui = 1 frame = 0 states = [] states.append(p.saveState()) #observations=[] #observations.append(obs) running = True