├── README.md ├── loadpanda_grasp.py └── panda_sim_grasp.py /README.md: -------------------------------------------------------------------------------- 1 | # panda_grasp_sim_1 2 | 3 | 基于pybullet官方提供的panda抓取示例改的，运行视频：https://www.bilibili.com/video/BV16A411W7fD 4 | 5 | 依赖包： 6 | numpy 7 | pybullet 8 | 9 | 10 | 通过以下命令运行： 11 | ``` 12 | python3 loadpanda_grasp.py 13 | ``` 14 | 15 | 这里提供的代码被我稍微修改了一点，运行效果和视频稍微有点差别。 16 | 17 | 任何问题可以联系dexinwang@mail.sdu.edu.cn 18 | -------------------------------------------------------------------------------- /loadpanda_grasp.py: -------------------------------------------------------------------------------- 1 | import pybullet as p 2 | import pybullet_data as pd 3 | import math 4 | import time 5 | import numpy as np 6 | import panda_sim_grasp as panda_sim 7 | 8 | 9 | fps=240. 10 | timeStep = 1./fps 11 | 12 | def run(): 13 | p.connect(p.GUI) 14 | 15 | # 设置仿真环境 16 | p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1) # 使Y轴向上, 默认Z轴向上 17 | # p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) 18 | p.setPhysicsEngineParameter(maxNumCmdPer1ms=1000) 19 | p.resetDebugVisualizerCamera(cameraDistance=1.3, cameraYaw=38, cameraPitch=-22, cameraTargetPosition=[0.35,-0.13,0]) 20 | p.setAdditionalSearchPath(pd.getDataPath()) 21 | p.setTimeStep(timeStep) 22 | p.setGravity(0,-9.8,0) 23 | 24 | # 初始化panda机器人 25 | panda = panda_sim.PandaSimAuto(p, [0,0,0]) 26 | panda.control_dt = timeStep 27 | 28 | # logId = panda.bullet_client.startStateLogging(panda.bullet_client.STATE_LOGGING_PROFILE_TIMINGS, "log.json") 29 | # panda.bullet_client.submitProfileTiming("start") 30 | 31 | graspWidthId = p.addUserDebugParameter("graspWidth", 0, 0.06, 0.01) 32 | 33 | for i in range (100000): 34 | # panda.bullet_client.submitProfileTiming("full_step") 35 | graspWidth = float(p.readUserDebugParameter(graspWidthId)) 36 | panda.step(graspWidth) 37 | p.stepSimulation() 38 | time.sleep(timeStep) 39 | # panda.bullet_client.submitProfileTiming() 40 | # panda.bullet_client.submitProfileTiming() 41 | # panda.bullet_client.stopStateLogging(logId) 42 | 43 | 44 | 45 | if __name__ == "__main__": 46 | run() 47 | -------------------------------------------------------------------------------- /panda_sim_grasp.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | import math 4 | 5 | useNullSpace = 1 6 | ikSolver = 0 7 | pandaEndEffectorIndex = 11 #8 8 | pandaNumDofs = 7 9 | 10 | ll = [-7]*pandaNumDofs 11 | #upper limits for null space (todo: set them to proper range) 12 | ul = [7]*pandaNumDofs 13 | #joint ranges for null space (todo: set them to proper range) 14 | jr = [7]*pandaNumDofs 15 | #restposes for null space 16 | jointPositions=[0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32, 0.02, 0.02] 17 | rp = jointPositions 18 | 19 | class PandaSim(object): 20 | def __init__(self, bullet_client, offset): 21 | self.bullet_client = bullet_client 22 | self.bullet_client.setPhysicsEngineParameter(solverResidualThreshold=0) 23 | self.offset = np.array(offset) 24 | 25 | #print("offset=",offset) 26 | flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES 27 | self.legos=[] 28 | 29 | self.bullet_client.loadURDF("tray/traybox.urdf", [0+offset[0], 0+offset[1], -0.6+offset[2]], [-0.5, -0.5, -0.5, 0.5], flags=flags) 30 | self.legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.5])+self.offset, flags=flags)) 31 | self.bullet_client.changeVisualShape(self.legos[0],-1,rgbaColor=[1,0,0,1]) 32 | self.legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([-0.1, 0.3, -0.5])+self.offset, flags=flags)) 33 | self.legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.1, 0.3, -0.7])+self.offset, flags=flags)) 34 | self.sphereId = self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.6])+self.offset, flags=flags) 35 | self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.5])+self.offset, flags=flags) 36 | # self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0, 0.3, -0.7])+self.offset, flags=flags) 37 | # self.cubeId = self.bullet_client.loadURDF("E:/research/dataset/grasp/EGAD/test_1/cube1.urdf",np.array( [0, 0.1, -0.7])+self.offset, flags=flags) 38 | orn=[-0.707107, 0.0, 0.0, 0.707107]#p.getQuaternionFromEuler([-math.pi/2,math.pi/2,0]) 39 | eul = self.bullet_client.getEulerFromQuaternion([-0.5, -0.5, -0.5, 0.5]) 40 | self.panda = self.bullet_client.loadURDF("franka_panda/panda.urdf", np.array([0,0,0])+self.offset, orn, useFixedBase=True, flags=flags) 41 | index = 0 42 | self.state = 0 43 | self.control_dt = 1./240. 44 | self.finger_target = 0 45 | self.gripper_height = 0.2 46 | #create a constraint to keep the fingers centered 47 | c = self.bullet_client.createConstraint(self.panda, 48 | 9, 49 | self.panda, 50 | 10, 51 | jointType=self.bullet_client.JOINT_GEAR, 52 | jointAxis=[1, 0, 0], 53 | parentFramePosition=[0, 0, 0], 54 | childFramePosition=[0, 0, 0]) 55 | self.bullet_client.changeConstraint(c, gearRatio=-1, erp=0.1, maxForce=50) 56 | 57 | for j in range(self.bullet_client.getNumJoints(self.panda)): 58 | self.bullet_client.changeDynamics(self.panda, j, linearDamping=0, angularDamping=0) 59 | info = self.bullet_client.getJointInfo(self.panda, j) 60 | #print("info=",info) 61 | jointName = info[1] 62 | jointType = info[2] 63 | if (jointType == self.bullet_client.JOINT_PRISMATIC): 64 | self.bullet_client.resetJointState(self.panda, j, jointPositions[index]) 65 | index=index+1 66 | 67 | if (jointType == self.bullet_client.JOINT_REVOLUTE): 68 | self.bullet_client.resetJointState(self.panda, j, jointPositions[index]) 69 | index=index+1 70 | self.t = 0. 71 | 72 | def reset(self): 73 | pass 74 | 75 | def update_state(self): 76 | keys = self.bullet_client.getKeyboardEvents() 77 | if len(keys)>0: 78 | for k,v in keys.items(): 79 | if v&self.bullet_client.KEY_WAS_TRIGGERED: 80 | if (k==ord('0')): 81 | self.state = 0 82 | if (k==ord('1')): 83 | self.state = 1 84 | if (k==ord('2')): 85 | self.state = 2 86 | if (k==ord('3')): # 物体上方 预抓取位置 87 | self.state = 3 88 | if (k==ord('4')): # 物体抓取位置 89 | self.state = 4 90 | if (k==ord('5')): # 机械手张开 91 | self.state = 5 92 | if (k==ord('6')): # 机械手闭合 93 | self.state = 6 94 | if (k==ord('7')): # 机械手闭合 95 | self.state = 7 96 | if v&self.bullet_client.KEY_WAS_RELEASED: 97 | self.state = 0 98 | def step(self, graspWidth): 99 | # 设置抓取器张开宽度 100 | if self.state==6: 101 | self.finger_target = 0.01 102 | if self.state==5: 103 | self.finger_target = 0.04 104 | 105 | # 测试用 106 | # self.finger_target = graspWidth 107 | # # print('self.finger_target = ', self.finger_target) 108 | # for i in [9,10]: 109 | # self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL,self.finger_target ,force= 10) 110 | 111 | self.update_state() 112 | # print("self.state=",self.state) 113 | #print("self.finger_target=",self.finger_target) 114 | 115 | alpha = 0.9 #0.99 116 | if self.state==1 or self.state==2 or self.state==3 or self.state==4 or self.state==7: 117 | #gripper_height = 0.034 118 | self.gripper_height = alpha * self.gripper_height + (1.-alpha)*0.03 119 | # print('self.gripper_height = ', self.gripper_height) 120 | 121 | if self.state == 2 or self.state == 3 or self.state == 7: 122 | self.gripper_height = alpha * self.gripper_height + (1.-alpha)*0.2 123 | # print('self.gripper_height = ', self.gripper_height) 124 | 125 | t = self.t 126 | self.t += self.control_dt 127 | 128 | pos = [self.offset[0]+0.2 * math.sin(1.5 * t), self.offset[1]+self.gripper_height, self.offset[2]+-0.6 + 0.1 * math.cos(1.5 * t)] # 圆形位置 129 | if self.state == 3 or self.state== 4: 130 | # 获取红色积木的位置和方向 131 | pos, o = self.bullet_client.getBasePositionAndOrientation(self.legos[0]) #sphereId self.legos[0] 132 | pos = [pos[0], self.gripper_height, pos[2]] # 机械手位置 133 | self.prev_pos = pos 134 | if self.state == 7: 135 | pos = self.prev_pos 136 | diffX = pos[0] - self.offset[0] 137 | diffZ = pos[2] - (self.offset[2]-0.6) 138 | self.prev_pos = [self.prev_pos[0] - diffX*0.1, self.prev_pos[1], self.prev_pos[2]-diffZ*0.1] 139 | 140 | 141 | orn = self.bullet_client.getQuaternionFromEuler([math.pi/2.,0.,0.]) # 机械手方向 142 | # 根据目标位置计算关节位置 143 | jointPoses = self.bullet_client.calculateInverseKinematics(self.panda, pandaEndEffectorIndex, pos, orn, ll, ul, jr, rp, maxNumIterations=20) 144 | 145 | for i in range(pandaNumDofs): 146 | self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL, jointPoses[i],force=5 * 240.) 147 | 148 | #target for fingers 149 | for i in [9,10]: 150 | self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL,self.finger_target ,force= 10) 151 | 152 | class PandaSimAuto(PandaSim): 153 | def __init__(self, bullet_client, offset): 154 | PandaSim.__init__(self, bullet_client, offset) 155 | self.state_t = 0 156 | self.cur_state = 0 157 | self.states=[0, 3, 5, 4, 6, 3, 7] 158 | self.state_durations=[3, 1, 1, 1, 1, 1, 1] 159 | 160 | def update_state(self): 161 | self.state_t += self.control_dt 162 | if self.state_t > self.state_durations[self.cur_state]: 163 | self.cur_state += 1 164 | if self.cur_state >= len(self.states): 165 | self.cur_state = 0 166 | self.state_t = 0 167 | self.state=self.states[self.cur_state] 168 | #print("self.state=",self.state) 169 | --------------------------------------------------------------------------------