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 _termination(self): #print (self._kuka.endEffectorPos[2]) state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] #print("self._envStepCounter") #print(self._envStepCounter) if (self.terminated or self._envStepCounter>1000): self._observation = self.getExtendedObservation() return True if (actualEndEffectorPos[2] <= 0.10): self.terminated = 1 #print("closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 for i in range (1000): graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) p.stepSimulation() fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 self._observation = self.getExtendedObservation() return True return False
def step2(self, action): for i in range(self._actionRepeat): self._kuka.applyAction(action) p.stepSimulation() if self._termination(): break self._envStepCounter += 1 if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() #print("self._envStepCounter") #print(self._envStepCounter) done = self._termination() npaction = np.array([action[3]]) #only penalize rotation until learning works well [action[0],action[1],action[3]]) actionCost = np.linalg.norm(npaction)*10. #print("actionCost") #print(actionCost) reward = self._reward()-actionCost #print("reward") #print(reward) #print("len=%r" % len(self._observation)) return np.array(self._observation), reward, done, {}
def buildJointNameToIdDict(self): nJoints = p.getNumJoints(self.quadruped) self.jointNameToId = {} for i in range(nJoints): jointInfo = p.getJointInfo(self.quadruped, i) self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] self.resetPose() for i in range(100): p.stepSimulation()
def applyAction(self, actions): forces = [0.] * len(self.motors) for m in range(len(self.motors)): forces[m] = self.motor_power[m]*actions[m]*0.082 p.setJointMotorControlArray(self.human, self.motors,controlMode=p.TORQUE_CONTROL, forces=forces) p.stepSimulation() time.sleep(0.01) distance=5 yaw = 0
def test(args): p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) fileName = os.path.join("mjcf", args.mjcf) print("fileName") print(fileName) p.loadMJCF(fileName) while (1): p.stepSimulation() p.getCameraImage(320,240) time.sleep(0.01)
def setupWorld(): p.resetSimulation() p.loadURDF("planeMesh.urdf") kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10]) for i in range (p.getNumJoints(kukaId)): p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0) for i in range (numObjects): cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2]) #p.changeDynamics(cube,-1,mass=100) p.stepSimulation() p.setGravity(0,0,-10)
def step(self,action): p.stepSimulation() start = time.time() yaw = next(self.iter) viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix, projectionMatrix, shadow=1,lightDirection=[1,1,1], renderer=p.ER_BULLET_HARDWARE_OPENGL) #renderer=pybullet.ER_TINY_RENDERER) self._observation = img_arr[2] return np.array(self._observation), 0, 0, {}
def reset(self): self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3) self.kp = 1 self.kd = 0.1 self.maxForce = 100 nJoints = p.getNumJoints(self.quadruped) self.jointNameToId = {} for i in range(nJoints): jointInfo = p.getJointInfo(self.quadruped, i) self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] self.resetPose() for i in range(100): p.stepSimulation()
def test(num_runs=300, shadow=1, log=True, plot=False): if log: logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings") if plot: plt.ion() img = np.random.rand(200, 320) #img = [tandard_normal((50,100)) image = plt.imshow(img,interpolation='none',animated=True,label="blah") ax = plt.gca() times = np.zeros(num_runs) yaw_gen = itertools.cycle(range(0,360,10)) for i, yaw in zip(range(num_runs),yaw_gen): pybullet.stepSimulation() start = time.time() viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix, projectionMatrix, shadow=shadow,lightDirection=[1,1,1], renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) #renderer=pybullet.ER_TINY_RENDERER) stop = time.time() duration = (stop - start) if (duration): fps = 1./duration #print("fps=",fps) else: fps=0 #print("fps=",fps) #print("duraction=",duration) #print("fps=",fps) times[i] = fps if plot: rgb = img_arr[2] image.set_data(rgb)#np_img_arr) ax.plot([0]) #plt.draw() #plt.show() plt.pause(0.01) mean_time = float(np.mean(times)) print("mean: {0} for {1} runs".format(mean_time, num_runs)) print("") if log: pybullet.stopStateLogging(logId) return mean_time
def _step(self, action): self._humanoid.applyAction(action) for i in range(self._actionRepeat): p.stepSimulation() if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() if self._termination(): break self._envStepCounter += 1 reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) return np.array(self._observation), reward, done, {}
def _step(self, action): p.stepSimulation() # time.sleep(self.timeStep) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state force = action p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(action + self.state[3])) done = x < -self.x_threshold \ or x > self.x_threshold \ or theta < -self.theta_threshold_radians \ or theta > self.theta_threshold_radians reward = 1.0 return np.array(self.state), reward, done, {}
def testJacobian(self): import pybullet as p clid = p.connect(p.SHARED_MEMORY) if (clid < 0): p.connect(p.DIRECT) time_step = 0.001 gravity_constant = -9.81 urdfs = [ "TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf", "kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf" ] for urdf in urdfs: p.resetSimulation() p.setTimeStep(time_step) p.setGravity(0.0, 0.0, gravity_constant) robotId = p.loadURDF(urdf, useFixedBase=True) p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1]) numJoints = p.getNumJoints(robotId) endEffectorIndex = numJoints - 1 # Set a joint target for the position control and step the sim. self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)]) p.stepSimulation() # Get the joint and link state directly from Bullet. mpos, mvel, mtorq = self.getMotorJointStates(robotId) result = p.getLinkState(robotId, endEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1) link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result # Get the Jacobians for the CoM of the end-effector link. # Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn. # The localPosition is always defined in terms of the link frame coordinates. zero_vec = [0.0] * len(mpos) jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec, zero_vec) assert (allclose(dot(jac_t, mvel), link_vt)) assert (allclose(dot(jac_r, mvel), link_vr)) p.disconnect()
def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=10000, sleepTime=0): print('start evaluation') beforeTime = time.time() p.resetSimulation() p.setTimeStep(timeStep) p.loadURDF("%s/plane.urdf" % urdfRoot) p.setGravity(0, 0, -10) global minitaur minitaur = Minitaur(urdfRoot) start_position = current_position() last_position = None # for tracing line total_energy = 0 for i in range(maxNumSteps): torques = minitaur.getMotorTorques() velocities = minitaur.getMotorVelocities() total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep joint_values = evaluate_func_map[evaluateFunc](i, params) minitaur.applyAction(joint_values) p.stepSimulation() if (is_fallen()): break if i % 100 == 0: sys.stdout.write('.') sys.stdout.flush() time.sleep(sleepTime) print(' ') alpha = objectiveParams[0] final_distance = np.linalg.norm(start_position - current_position()) finalReturn = final_distance - alpha * total_energy elapsedTime = time.time() - beforeTime print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy, "finalReturn", finalReturn, "elapsed_time", elapsedTime) return finalReturn
def setupWorld(self): numObjects = 50 maximalCoordinates = False p.resetSimulation() p.setPhysicsEngineParameter(deterministicOverlappingPairs=1) p.loadURDF("planeMesh.urdf", useMaximalCoordinates=maximalCoordinates) kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10], useMaximalCoordinates=maximalCoordinates) for i in range(p.getNumJoints(kukaId)): p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0) for i in range(numObjects): cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2]) #p.changeDynamics(cube,-1,mass=100) p.stepSimulation() p.setGravity(0, 0, -10)
def _step(self, action): force = self.force_mag if action==1 else -self.force_mag p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force) p.stepSimulation() self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state done = x < -self.x_threshold \ or x > self.x_threshold \ or theta < -self.theta_threshold_radians \ or theta > self.theta_threshold_radians done = bool(done) reward = 1.0 #print("state=",self.state) return np.array(self.state), reward, done, {}
def _termination(self): #print (self._kuka.endEffectorPos[2]) state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] #print("self._envStepCounter") #print(self._envStepCounter) if (self.terminated or self._envStepCounter>self._maxSteps): self._observation = self.getExtendedObservation() return True maxDist = 0.005 closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 #print("terminating, closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 for i in range (100): graspAction = [0,0,0.0001,0,fingerAngle] self._kuka.applyAction(graspAction) p.stepSimulation() fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 for i in range (1000): graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) p.stepSimulation() blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) if (blockPos[2] > 0.23): #print("BLOCKPOS!") #print(blockPos[2]) break state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] if (actualEndEffectorPos[2]>0.5): break self._observation = self.getExtendedObservation() return True return False
def _step(self, action): p.stepSimulation() # time.sleep(self.timeStep) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state dv = 0.1 deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action] p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3])) done = x < -self.x_threshold \ or x > self.x_threshold \ or theta < -self.theta_threshold_radians \ or theta > self.theta_threshold_radians reward = 1.0 return np.array(self.state), reward, done, {}
def test_rolling_friction(self): import pybullet as p p.connect(p.DIRECT) p.loadURDF("plane.urdf") sphere = p.loadURDF("sphere2.urdf", [0, 0, 1]) p.resetBaseVelocity(sphere, linearVelocity=[1, 0, 0]) p.changeDynamics(sphere, -1, linearDamping=0, angularDamping=0) #p.changeDynamics(sphere,-1,rollingFriction=0) p.setGravity(0, 0, -10) for i in range(1000): p.stepSimulation() vel = p.getBaseVelocity(sphere) self.assertLess(vel[0][0], 1e-10) self.assertLess(vel[0][1], 1e-10) self.assertLess(vel[0][2], 1e-10) self.assertLess(vel[1][0], 1e-10) self.assertLess(vel[1][1], 1e-10) self.assertLess(vel[1][2], 1e-10) p.disconnect()
def _reset(self): p.resetSimulation() #p.setPhysicsEngineParameter(numSolverIterations=300) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf")) dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 p.setGravity(0,0,-10) self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation)
def step2(self, action): for i in range(self._actionRepeat): self._kuka.applyAction(action) p.stepSimulation() if self._termination(): break #self._observation = self.getExtendedObservation() self._envStepCounter += 1 self._observation = self.getExtendedObservation() if self._renders: time.sleep(self._timeStep) #print("self._envStepCounter") #print(self._envStepCounter) done = self._termination() reward = self._reward() #print("len=%r" % len(self._observation)) return np.array(self._observation), reward, done, {}
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)
def _reset(self): """Environment reset called at the beginning of an episode. """ # Set the camera settings. look = [0.23, 0.2, 0.54] distance = 1. pitch = -56 + self._cameraRandom*np.random.uniform(-3, 3) yaw = 245 + self._cameraRandom*np.random.uniform(-3, 3) roll = 0 self._view_matrix = p.computeViewMatrixFromYawPitchRoll( look, distance, yaw, pitch, roll, 2) fov = 20. + self._cameraRandom*np.random.uniform(-2, 2) aspect = self._width / self._height near = 0.01 far = 10 self._proj_matrix = p.computeProjectionMatrixFOV( fov, aspect, near, far) self._attempted_grasp = False self._env_step = 0 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) p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() # Choose the objects in the bin. urdfList = self._get_random_object( self._numObjects, self._isTest) self._objectUids = self._randomly_place_objects(urdfList) self._observation = self._get_observation() return np.array(self._observation)
def main(unused_args): timeStep = 0.01 c = p.connect(p.SHARED_MEMORY) if (c<0): c = p.connect(p.GUI) p.resetSimulation() p.setTimeStep(timeStep) p.loadURDF("plane.urdf") p.setGravity(0,0,-10) minitaur = Minitaur() amplitude = 0.24795664427 speed = 0.2860877729434 for i in range(1000): a1 = math.sin(i*speed)*amplitude+1.57 a2 = math.sin(i*speed+3.14)*amplitude+1.57 joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57] minitaur.applyAction(joint_values) p.stepSimulation() # print(minitaur.getBasePosition()) time.sleep(timeStep) final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition())) print(final_distance)
def _step_continuous(self, action): """Applies a continuous velocity-control action. Args: action: 5-vector parameterizing XYZ offset, vertical angle offset (radians), and grasp angle (radians). Returns: observation: Next observation. reward: Float of the per-step reward as a result of taking the action. done: Bool of whether or not the episode has ended. debug: Dictionary of extra information provided by environment. """ # Perform commanded action. self._env_step += 1 self._kuka.applyAction(action) for _ in range(self._actionRepeat): p.stepSimulation() if self._renders: time.sleep(self._timeStep) if self._termination(): break # If we are close to the bin, attempt grasp. state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex) end_effector_pos = state[0] if end_effector_pos[2] <= 0.1: finger_angle = 0.3 for _ in range(500): grasp_action = [0, 0, 0, 0, finger_angle] self._kuka.applyAction(grasp_action) p.stepSimulation() #if self._renders: # time.sleep(self._timeStep) finger_angle -= 0.3/100. if finger_angle < 0: finger_angle = 0 for _ in range(500): grasp_action = [0, 0, 0.001, 0, finger_angle] self._kuka.applyAction(grasp_action) p.stepSimulation() if self._renders: time.sleep(self._timeStep) finger_angle -= 0.3/100. if finger_angle < 0: finger_angle = 0 self._attempted_grasp = True observation = self._get_observation() done = self._termination() reward = self._reward() debug = { 'grasp_success': self._graspSuccess } return observation, reward, done, debug
parser = urdfEditor.UrdfEditor() parser.initializeFromBulletBody(mb,physicsClientId=org) parser.saveUrdf("test.urdf") if (1): print("\ncreatingMultiBody...................n") obUid = parser.createMultiBody(physicsClientId=gui) parser2 = urdfEditor.UrdfEditor() print("\n###########################################\n") parser2.initializeFromBulletBody(obUid,physicsClientId=gui) parser2.saveUrdf("test2.urdf") for i in range (p.getNumJoints(obUid, physicsClientId=gui)): p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0,force=1,physicsClientId=gui) print(p.getJointInfo(obUid,i,physicsClientId=gui)) parser=0 p.setRealTimeSimulation(1,physicsClientId=gui) while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]): p.stepSimulation(physicsClientId=gui) time.sleep(0.01)
def _step_continuous(self, action): """Applies a continuous velocity-control action. Args: action: 5-vector parameterizing XYZ offset, vertical angle offset (radians), and grasp angle (radians). Returns: observation: Next observation. reward: Float of the per-step reward as a result of taking the action. done: Bool of whether or not the episode has ended. debug: Dictionary of extra information provided by environment. """ # Perform commanded action. self._env_step += 1 self._kuka.applyAction(action) for _ in range(self._actionRepeat): p.stepSimulation() time.sleep(0.00001) state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex) end_effector_pos = state[0] if self._termination(): break # If we are close to the bin, attempt grasp. state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex) end_effector_pos = state[0] if end_effector_pos[2] <= 0.1:#0.1 for prev gripper time.sleep(1) print("attempting grasp") print(end_effector_pos) motor_fing = np.pi/4 hinge_fing = -np.pi/6 #grab config grab = np.array([motor_fing,hinge_fing,-motor_fing,hinge_fing]) #grab = np.array([0,0,0,0]) for _ in range(500): grasp_action = [0, 0, 0, 0, grab] self._kuka.applyAction(grasp_action) p.stepSimulation() #if self._renders: #time.sleep(self._timeStep) #finger_angle -= 0.3 / 100. #if finger_angle < 0: #finger_angle = 0 for _ in range(100000): grasp_action = [0, 0, 0.001, 0, grab] self._kuka.applyAction(grasp_action) p.stepSimulation() state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex) end_effector_pos = state[0] if end_effector_pos[2]>0.8: break time.sleep(self._timeStep) #finger_angle -= 0.3 / 100. #if finger_angle < 0: #finger_angle = 0 for _ in range(1000): grasp_action = [0, 0.01, 0.005, 0, grab] self._kuka.applyAction(grasp_action) p.stepSimulation() time.sleep(self._timeStep) #finger_angle -= 0.3 / 100. #finger_angle=0.3 for _ in range(10000): grasp_action = [0, -0.01, -0.005, 0, grab] self._kuka.applyAction(grasp_action) p.stepSimulation() time.sleep(self._timeStep) for _ in range(1000): grasp_action = [0, 0.01, -0.001, 0, [0,0,0,0]] self._kuka.applyAction(grasp_action) p.stepSimulation() time.sleep(self._timeStep) self._attempted_grasp = True observation = self._get_observation() done = self._termination() reward = self._reward() for body in self._removal: p.removeBody(body) self._objectUids.remove(body) self._removal=[] for _ in range(100): grasp_action=[0,0,0,0,np.zeros((4))] self._kuka.applyAction(grasp_action) debug = {'grasp_success': self._graspSuccess} return observation, reward, done, debug
print(obj_vel) print("obj_acc") print(obj_acc) torque = bullet.calculateInverseDynamics(id_robot, obj_pos, obj_vel, obj_acc) q_tor[0][i] = torque[0] q_tor[1][i] = torque[1] if (verbose): print("torque=") print(torque) # Set the Joint Torques: bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.TORQUE_CONTROL, forces=[torque[0], torque[1]]) # Step Simulation bullet.stepSimulation() # Plot the Position, Velocity and Acceleration: if plot: figure = plt.figure(figsize=[15, 4.5]) figure.subplots_adjust(left=0.05, bottom=0.11, right=0.97, top=0.9, wspace=0.4, hspace=0.55) ax_pos = figure.add_subplot(141) ax_pos.set_title("Joint Position") ax_pos.plot(t, q_pos_desired[0], '--r', lw=4, label='Desired q0') ax_pos.plot(t, q_pos_desired[1], '--b', lw=4, label='Desired q1') ax_pos.plot(t, q_pos[0], '-r', lw=1, label='Measured q0') ax_pos.plot(t, q_pos[1], '-b', lw=1, label='Measured q1') ax_pos.set_ylim(-1., 1.) ax_pos.legend()
def reset(self): if (self.test_phase): global test_steps,test_done if (test_steps != 0 ): self.save_data_test() test_steps = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) self._envStepCounter = 0 self.ep_counter = self.ep_counter + 1 p.loadURDF(os.path.join(pybullet_data.getDataPath(),"plane.urdf"), useFixedBase= True) # Load robot self._panda = pandaEnv(self._urdfRoot, timeStep=self._timeStep, basePosition =[0,0,0.625], useInverseKinematics= self._useIK, action_space = self.action_dim, includeVelObs = self.includeVelObs) # Load table and object for simulation tableId = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/table.urdf"), useFixedBase=True) table_info = p.getVisualShapeData(tableId,-1)[0] self._h_table =table_info[5][2] + table_info[3][2] #limit panda workspace to table plane self._panda.workspace_lim[2][0] = self._h_table # Randomize start position of object and target. #we take the target point if (self.fixedPositionObj): if(self.object_position==0): #we have completely fixed position self.obj_pose = [0.6,0.1,0.64] self.target_pose = [0.4,0.45,0.64] self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition = self.obj_pose) self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose) elif(self.object_position==1): if (self.keep_fixed_position): if (self.ep_counter == 100): self.obj_pose = [np.random.uniform(0.5,0.6),np.random.uniform(0,0.1),0.64] self.target_pose = [np.random.uniform(0.4,0.5),np.random.uniform(0.45,0.55),0.64] self.ep_counter = -1 else: self.obj_pose = [np.random.uniform(0.5,0.6),np.random.uniform(0,0.1),0.64] self.target_pose = self.target_pose = [0.4,0.45,0.64] #self.target_pose = [np.random.uniform(0.4,0.5),np.random.uniform(0.45,0.55),0.64] self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition = self.obj_pose) self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose) elif(self.object_position==2): self.obj_pose = [np.random.uniform(0.4,0.6),np.random.uniform(0,0.2),0.64] self.target_pose = [np.random.uniform(0.3,0.5),np.random.uniform(0.35,0.55),0.64] self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition = self.obj_pose) self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose) elif(self.object_position==3): print("") else: self.obj_pose, self.target_pose = self._sample_pose() self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition= self.obj_pose) #useful to see where is the taget point self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose) self._debugGUI() p.setGravity(0,0,-9.8) # Let the world run for a bit for _ in range(10): p.stepSimulation() #we take the dimension of the observation return self.getExtendedObservation()
def move_bot(botId, goal_pos, goal_dir, steps=500, reset=False): goalJoints = p.calculateInverseKinematics( botId, 9, goal_pos, targetOrientation=goal_dir, lowerLimits=joint_ll, upperLimits=joint_ul, jointRanges=joint_jr, restPoses=[0, 0, 0, 1, -0.1, 0, 0, 0.1, 0], jointDamping=[ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ]) numJoints = p.getNumJoints(botId) def set_joints(reset_joints): for ji in range(numJoints): jointInfo = p.getJointInfo(botId, ji) qIndex = jointInfo[3] if qIndex > -1: x = goalJoints[qIndex - 7] if qIndex - 7 < len(joint_ll) and (x < joint_ll[qIndex - 7] or x > joint_ul[qIndex - 7]): print('LIMIT VIOLATION!!', x, joint_ll[qIndex - 7], joint_ul[qIndex - 7], jointInfo[1]) #exit() if reset_joints: p.resetJointState(botId, ji, x, 0) p.setJointMotorControl2(botId, ji, p.POSITION_CONTROL, targetPosition=x, targetVelocity=0, force=500, positionGain=0.03, velocityGain=1) def check_joints(goalJoints): jointStates = [] jointNames = [] if goalJoints is not None: numJoints = p.getNumJoints(botId) for ji in range(numJoints): jointInfo = p.getJointInfo(botId, ji) qIndex = jointInfo[3] if qIndex > -1: x = goalJoints[qIndex - 7] jointPos = p.getJointState(botId, ji)[0] jointNames.append(jointInfo[1]) jointStates.append(jointPos) return jointStates, jointNames set_joints(False) stats = [[] for _ in range(len(goalJoints))] for i in range(steps): if reset: print('setting joints') set_joints(True) p.stepSimulation() if i % 10 == 0: pos, names = check_joints(goalJoints) for pi, px in enumerate(pos): stats[pi].append(px) import matplotlib.pyplot as plt fig, ax = plt.subplots(1, 10) for i, (g, s) in enumerate(zip(goalJoints, stats)): ax[i].set_title(str(names[i])) ax[i].plot(s, label='actual') ax[i].plot([0, 100], [g, g], label='target') if i < len(joint_ul): ax[i].plot([0, 100], [joint_ll[i], joint_ll[i]], label='lower') ax[i].plot([0, 100], [joint_ul[i], joint_ul[i]], label='upper') fig.set_size_inches(30, 2) fig.savefig('joints.png')
import pybullet as p import time import pybullet_data physics_client = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -9.8) robotStartPos = [0, 0, 0.15] robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) planeID = p.loadURDF("plane.urdf") robotID = p.loadURDF("mobile.urdf", robotStartPos, robotStartOrientation) for i in range(10000): p.stepSimulation() time.sleep(1. / 240.) cubePos, cubeOrn = p.getBasePositionAndOrientation(robotID) print(cubePos, cubeOrn) p.disconnect()
def reset(self): self.terminated = False self.n_contacts = 0 self.n_steps_outside = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timestep) p.loadURDF(os.path.join(self._urdf_root, "plane.urdf"), [0, 0, -1]) self.table_uid = p.loadURDF( os.path.join(self._urdf_root, "table/table.urdf"), 0.5000000, 0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0) # Initialize button position x_pos = 0.5 y_pos = 0 if self._random_target: x_pos += 0.15 * self.np_random.uniform(-1, 1) y_pos += 0.3 * self.np_random.uniform(-1, 1) self.button_uid = p.loadURDF("/urdf/simple_button.urdf", [x_pos, y_pos, Z_TABLE]) self.button_pos = np.array([x_pos, y_pos, Z_TABLE]) p.setGravity(0, 0, -10) self._kuka = kuka.Kuka(urdf_root_path=self._urdf_root, timestep=self._timestep, use_inverse_kinematics=(not self.action_joints), small_constraints=(not self._random_target)) self._env_step_counter = 0 # Close the gripper and wait for the arm to be in rest position for _ in range(500): if self.action_joints: self._kuka.applyAction( list(np.array(self._kuka.joint_positions)[:7]) + [0, 0]) else: self._kuka.applyAction([0, 0, 0, 0, 0]) p.stepSimulation() # Randomize init arm pos: take 5 random actions for _ in range(N_RANDOM_ACTIONS_AT_INIT): if self._is_discrete: action = [0, 0, 0, 0, 0] sign = 1 if self.np_random.rand() > 0.5 else -1 action_idx = self.np_random.randint(3) # dx, dy or dz action[action_idx] += sign * DELTA_V else: if self.action_joints: joints = np.array(self._kuka.joint_positions)[:7] joints += DELTA_THETA * self.np_random.normal(joints.shape) action = list(joints) + [0, 0] else: action = np.zeros(5) rand_direction = self.np_random.normal((3, )) # L2 normalize, so that the random direction is not too high or too low rand_direction /= np.linalg.norm(rand_direction, 2) action[:3] += DELTA_V_CONTINUOUS * rand_direction self._kuka.applyAction(list(action)) p.stepSimulation() self._observation = self.getExtendedObservation() self.button_pos = np.array( p.getLinkState(self.button_uid, BUTTON_LINK_IDX)[0]) self.button_pos[ 2] += BUTTON_DISTANCE_HEIGHT # Set the target position on the top of the button if self.saver is not None: self.saver.reset(self._observation, self.getTargetPos(), self.getGroundTruth()) if self.srl_model != "raw_pixels": return self.getSRLState(self._observation) return np.array(self._observation)
def testSaveRestoreState(self): numSteps = 500 numSteps2 = 30 verbose = 0 self.setupWorld() for i in range(numSteps): p.stepSimulation() p.saveBullet("state.bullet") if verbose: p.setInternalSimFlags(1) p.stepSimulation() if verbose: p.setInternalSimFlags(0) print("contact points=") for q in p.getContactPoints(): print(q) for i in range(numSteps2): p.stepSimulation() file = open("saveFile.txt", "w") self.dumpStateToFile(file) file.close() ################################# self.setupWorld() #both restore from file or from in-memory state should work p.restoreState(fileName="state.bullet") stateId = p.saveState() if verbose: p.setInternalSimFlags(1) p.stepSimulation() if verbose: p.setInternalSimFlags(0) print("contact points restored=") for q in p.getContactPoints(): print(q) for i in range(numSteps2): p.stepSimulation() file = open("restoreFile.txt", "w") self.dumpStateToFile(file) file.close() p.restoreState(stateId) if verbose: p.setInternalSimFlags(1) p.stepSimulation() if verbose: p.setInternalSimFlags(0) print("contact points restored=") for q in p.getContactPoints(): print(q) for i in range(numSteps2): p.stepSimulation() file = open("restoreFile2.txt", "w") self.dumpStateToFile(file) file.close() file1 = open("saveFile.txt", "r") file2 = open("restoreFile.txt", "r") self.compareFiles(file1, file2) file1.close() file2.close() file1 = open("saveFile.txt", "r") file2 = open("restoreFile2.txt", "r") self.compareFiles(file1, file2) file1.close() file2.close()
def playReferenceMotion(self, motionFile): motionFile = os.path.join(os.path.dirname(__file__), motionFile) with open(motionFile, "r") as motion_file: motion = json.load(motion_file) ''' Joint indices should correspond to the following: 0 - root Type: FIXED 1 - chest Type: SPHERICAL 2 - neck Type: SPHERICAL 3 - right_hip Type: SPHERICAL 4 - right_knee Type: REVOLUTE 5 - right_ankle Type: SPHERICAL 6 - right_shoulder Type: SPHERICAL 7 - right_elbow Type: REVOLUTE 8 - right_wrist Type: FIXED 9 - left_hip Type: SPHERICAL 10 - left_knee Type: REVOLUTE 11 - left_ankle Type: SPHERICAL 12 - left_shoulder Type: SPHERICAL 13 - left_elbow Type: REVOLUTE 14 - left_wrist Type: FIXED ''' JointFrameMapIndices = [ 0, #root [9, 10, 11, 8], #chest [13, 14, 15, 12], #neck [17, 18, 19, 16], #rHip 20, #rKnee [22, 23, 24, 21], #rAnkle [26, 27, 28, 25], #rShoulder 29, #rElbow 0, #rWrist [31, 32, 33, 30], #lHip 34, #lKnee [36, 37, 38, 35], #lAnkle [40, 41, 42, 39], #lShoulder 43, #lElbow 0, #lWrist ] for frame in motion['Frames']: basePos1 = [frame[i] for i in [1, 2, 3]] baseOrn1 = frame[5:8] + [frame[4]] # transform the position and orientation to have z-axis upward y2zPos = [0, 0, 0.0] y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0]) basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1) # set the agent's root position and orientation p.resetBasePositionAndOrientation( self.humanoidAgent, posObj=basePos, ornObj=baseOrn ) # Set joint positions for joint in self.revoluteJoints: p.resetJointState( self.humanoidAgent, jointIndex=joint, targetValue=frame[JointFrameMapIndices[joint]] ) for joint in self.sphericalJoints: p.resetJointStateMultiDof( self.humanoidAgent, jointIndex=joint, targetValue=[frame[i] for i in JointFrameMapIndices[joint]] ) for i in range(16): p.stepSimulation() time.sleep(0.1/240)
def step(self, ctrl_raw): # Add new action to queue self.act_queue.append(ctrl_raw) self.act_queue.pop(0) # Simulate the delayed action if self.randomized_params["output_transport_delay"] > 0: ctrl_raw_unqueued = self.act_queue[-self.config["act_input"]:] ctrl_delayed = self.act_queue[ -1 - self.randomized_params["output_transport_delay"]] else: ctrl_raw_unqueued = self.act_queue ctrl_delayed = self.act_queue[-1] # Scale the action appropriately (neural network gives [-1,1], pid controller [0,1]) if self.config["controller_source"] == "nn": ctrl_processed = np.clip(ctrl_delayed, -1, 1) * 0.5 + 0.5 else: ctrl_processed = ctrl_delayed # Take into account motor delay self.update_motor_vel(ctrl_processed) # Apply motor forces for i in range(4): motor_force_w_noise = np.clip( self.current_motor_velocity_vec[i] * self.randomized_params["motor_power_variance_vector"][i], 0, 1) motor_force_scaled = motor_force_w_noise * self.randomized_params[ "motor_force_multiplier"] p.applyExternalForce(self.robot, linkIndex=i * 2 + 1, forceObj=[0, 0, motor_force_scaled], posObj=[0, 0, 0], flags=p.LINK_FRAME, physicsClientId=self.client_ID) p.applyExternalTorque( self.robot, linkIndex=i * 2 + 1, torqueObj=[ 0, 0, self.current_motor_velocity_vec[i] * self.reactive_torque_dir_vec[i] * self.config["propeller_parasitic_torque_coeff"] ], flags=p.LINK_FRAME, physicsClientId=self.client_ID) self.apply_external_disturbances() p.stepSimulation(physicsClientId=self.client_ID) if self.config["animate"]: time.sleep(self.config["sim_timestep"]) self.step_ctr += 1 torso_pos, torso_quat, torso_euler, torso_vel, torso_angular_vel = self.get_obs( ) roll, pitch, yaw = torso_euler pos_delta = np.array(torso_pos) - np.array(self.config["target_pos"]) crashed = (abs(pos_delta) > 6.0).any() or ( (torso_pos[2] < 0.3) and (abs(roll) > 2.5 or abs(pitch) > 2.5)) if self.prev_act is not None: action_penalty = np.mean( np.square(np.array(ctrl_processed) - np.array(self.prev_act)) ) * self.config["pen_act_coeff"] else: action_penalty = 0 # Calculate true reward pen_position = np.mean(np.square(pos_delta)) * ( self.config["pen_position_coeff"] + 0 * self.step_ctr / float(self.config["max_steps"])) pen_rpy = np.mean(np.square( np.array(torso_euler))) * self.config["pen_rpy_coeff"] pen_vel = np.mean(np.square(torso_vel)) * self.config["pen_vel_coeff"] pen_rotvel = np.mean( np.square(torso_angular_vel)) * self.config["pen_ang_vel_coeff"] r_true = -action_penalty - pen_position - pen_rpy - pen_rotvel - pen_vel #print(action_penalty, pen_position, pen_rpy, pen_rotvel, pen_vel) # Calculate proxy reward (for learning purposes) pen_position_proxy = np.mean( np.square(pos_delta)) * self.config["pen_position_coeff"] pen_yaw_proxy = np.mean(np.square(yaw)) * self.config["pen_yaw_coeff"] pen_vel_proxy = np.mean(np.square(torso_vel)) * np.maximum( 1. - 3 * pen_position_proxy, 0) * self.config["pen_vel_coeff"] r = -pen_position_proxy - pen_yaw_proxy - action_penalty - pen_vel_proxy r = np.clip(r_true, -2, 2) if self.step_ctr == 1: r = 0 self.rew_queue.append([r]) self.rew_queue.pop(0) if self.randomized_params["input_transport_delay"] > 0: r_unqueued = self.rew_queue[-self.config["rew_input"]:] else: r_unqueued = self.rew_queue done = (self.step_ctr > self.config["max_steps"]) compiled_obs = pos_delta, torso_quat, torso_vel, torso_angular_vel compiled_obs_flat = [ item for sublist in compiled_obs for item in sublist ] self.obs_queue.append(compiled_obs_flat) self.obs_queue.pop(0) if self.randomized_params["input_transport_delay"] > 0: obs_raw_unqueued = self.obs_queue[-self.config["obs_input"]:] else: obs_raw_unqueued = self.obs_queue aux_obs = [] for i in range(len(obs_raw_unqueued)): t_obs = [] t_obs.extend(obs_raw_unqueued[i]) if self.config["act_input"] > 0: t_obs.extend(ctrl_raw_unqueued[i]) if self.config["rew_input"] > 0: t_obs.extend(r_unqueued[i]) if self.config["step_counter"] > 0: def step_ctr_to_obs(step_ctr): return (float(step_ctr) / self.config["max_steps"]) * 2 - 1 t_obs.extend( [step_ctr_to_obs(np.maximum(self.step_ctr - i - 1, 0))]) aux_obs.extend(t_obs) if self.config["latent_input"]: aux_obs.extend(self.randomized_params_list_norm) obs = np.array(aux_obs).astype(np.float32) obs_dict = { "pos_delta": pos_delta, "torso_quat": torso_quat, "torso_vel": torso_vel, "torso_angular_vel": torso_angular_vel, "reward": r_true, "action": ctrl_processed } self.prev_act = ctrl_processed return obs, r, done, { "obs_dict": obs_dict, "randomized_params": self.randomized_params, "true_rew": r_true }
robot = Robot() robot_viz = RobotViz(sim) robot_viz.load([0.0, 0.0, robot.height, 0.0]) path = Path2D(trajectory) path_viz = PathViz(sim) path_viz.load_path(path) trajectory_end = False current_tf = robot_viz.current_tf() while not trajectory_end: goal_tf = path.get_current_target() delta_tf = get_control_cmd(pbtf2pathtf(current_tf), goal_tf) new_tf = robot.move(current_tf, delta_tf) robot_viz.visualize_base(*new_tf) p.stepSimulation(sim.pClient) current_tf = robot_viz.current_tf() is_success, trajectory_end = path.try_step(pbtf2pathtf(current_tf)) if is_success: lx, ly, ltheta = pbtf2pathtf(current_tf) gx, gy, gtheta = path.get_current_target() path_viz.draw_line(fx=lx, fy=ly, tx=gx, ty=gy) # TODO add visualization that target is reached # time.sleep(1/30) time.sleep(1 / 10) # TRAJECTORY is OVER - rotate robot for i in range(20): current_tf = robot_viz.current_tf() delta_tf = 0.0, 0.0, 0.5 new_tf = robot.move(current_tf, delta_tf)
def run(self, runtime, goal_position=[]): if (not goal_position == []) and (not self.goal_position == goal_position): # if self.goalBodyID is not None: # pybullet.removeBody(self.goalBodyID, physicsClientId=self.physicsClient) self.goalBodyID = pybullet.createMultiBody( baseMass=0.0, baseInertialFramePosition=[0, 0, 0], baseVisualShapeIndex=self.visualGoalShapeId, basePosition=goal_position, useMaximalCoordinates=True, physicsClientId=self.physicsClient) # print ("Goal information: ", self.goalBodyID) self.__base_collision = False self.__heightExceed = False self.goal_position = goal_position assert not self.__controller == None, "Controller not set" self.__states = [self.getState()] #load with init state self.__rotations = [self.getRotation()] self.__commands = [] old_time = 0 first = True self.__command = object controlStep = 0 self.flip = False for i in range(int(runtime / self.__simStep)): if i * self.__simStep - old_time > self.__controlStep or first: state = self.getState() self.__command = self.__controller.nextCommand(i * self.__simStep) if not first: self.__states.append(state) self.__rotations.append(self.getRotation()) self.__commands.append(self.__command) first = False old_time = i * self.__simStep controlStep += 1 self.set_commands(self.__command) pybullet.stepSimulation(physicsClientId=self.physicsClient) if pybullet.getConnectionInfo( self.physicsClient)['connectionMethod'] == pybullet.GUI: time.sleep(self.__simStep / float(self.__vspeed)) # Flipfing behavior if self.getZOrientation() < 0.0: self.flip = True #Base floor collision if len( pybullet.getContactPoints( self.__planeId, self.__hexapodId, -1, -1, physicsClientId=self.physicsClient)) > 0: self.__base_collision = True #Jumping behavior when CM crosses 2.2 if self.getState()[2] > 2.2: self.__heightExceed = True
def move_pos( self, absolute_pos=None, relative_pos=None, absolute_global_euler=None, # preferred relative_global_euler=None, # preferred relative_local_euler=None, # not using absolute_global_quat=None, # preferred relative_azi=None, # for arm # relative_quat=None, # never use relative quat numSteps=50, maxJointVel=0.20, timeStep=0, relativePos=True, checkContact=False, objId=None, posGain=20, velGain=5): # Get trajectory eePosNow, eeQuatNow = self._panda.get_ee() # Determine target pos if absolute_pos is not None: targetPos = absolute_pos elif relative_pos is not None: targetPos = eePosNow + relative_pos else: targetPos = eePosNow # Determine target orn if absolute_global_euler is not None: targetOrn = euler2quat(absolute_global_euler) elif relative_global_euler is not None: targetOrn = quatMult(euler2quat(relative_global_euler), eeQuatNow) elif relative_local_euler is not None: targetOrn = quatMult(eeQuatNow, euler2quat(relative_local_euler)) elif absolute_global_quat is not None: targetOrn = absolute_global_quat elif relative_azi is not None: # Extrinsic yaw targetOrn = quatMult(euler2quat([relative_azi[0], 0, 0]), eeQuatNow) # Intrinsic pitch targetOrn = quatMult(targetOrn, euler2quat([0, relative_azi[1], 0])) # elif relative_quat is not None: # targetOrn = quatMult(eeQuatNow, relative_quat) else: targetOrn = array([1.0, 0., 0., 0.]) # Get trajectory trajPos = self.traj_time_scaling(startPos=eePosNow, endPos=targetPos, numSteps=numSteps) # Run steps numSteps = len(trajPos) for step in range(numSteps): # Get joint velocities from error tracking control, takes 0.2ms jointDot = self.traj_tracking_vel(targetPos=trajPos[step], targetQuat=targetOrn, posGain=posGain, velGain=velGain) # Send velocity commands to joints for i in range(self._panda.numJointsArm): p.setJointMotorControl2(self._pandaId, i, p.VELOCITY_CONTROL, targetVelocity=jointDot[i], force=self._panda.maxJointForce[i], maxVelocity=maxJointVel) # Keep gripper current velocity p.setJointMotorControl2(self._pandaId, self._panda.pandaLeftFingerJointIndex, p.VELOCITY_CONTROL, targetVelocity=self._panda.fingerCurVel, force=self._panda.maxFingerForce, maxVelocity=0.05) p.setJointMotorControl2(self._pandaId, self._panda.pandaRightFingerJointIndex, p.VELOCITY_CONTROL, targetVelocity=self._panda.fingerCurVel, force=self._panda.maxFingerForce, maxVelocity=0.05) # Quit if contact at either finger if checkContact: contact = self.check_contact(objId, both=False) if contact: return timeStep, False # Step simulation, takes 1.5ms p.stepSimulation() timeStep += 1 return timeStep, True
def velocity(self, value): Actuator.setVelocity(self.joint, value) @property def torque(self): jointState = self.joint.getState() return jointState.appliedTorque @torque.setter def torque(self, value): Actuator.setTorque(self.joint, value) if __name__ == "__main__": import os sid = pb.connect(pb.GUI) cwd = os.getcwd() pb.setAdditionalSearchPath(cwd + "../../data") pb.setGravity(0, 0, -9.8, sid) robot = BaxterRobot() robot.resetJoints() pb.setRealTimeSimulation(True) for i in range(1000000): pb.stepSimulation() robot.controlActuators({"left_e0": 1.0}, controlType="torque") print(robot.actuators.keys()) print(robot.joints.keys())
def step_simulation(steps=1, simulate=False): for i in range(steps): p.stepSimulation() if simulate: time.sleep(SLEEP)
def test_foot_pressure_synchronization(self): import pybullet as pb fig, axs = plt.subplots(2) self.walker.setPose(Transformation([0, 0, 0], [0, 0, 0, 1])) self.walker.ready() self.walker.wait(100) self.walker.setGoal(Transformation([1, 0, 0], [0, 0, 0, 1])) times = np.linspace(0, self.walker.soccerbot.robot_path.duration(), num=math.ceil(self.walker.soccerbot.robot_path.duration() / self.walker.soccerbot.robot_path.step_size) + 1) lfp = np.zeros((4, 4, len(times))) rfp = np.zeros((4, 4, len(times))) i = 0 for t in times: [lfp[:,:, i], rfp[:,:, i]] = self.walker.soccerbot.robot_path.footPosition(t) i = i + 1 right = rfp[2, 3, :].ravel() left = lfp[2, 3, :].ravel() right = 0.1 - right left = left - 0.1 axs[1].plot(times, left, label='Left') axs[1].plot(times, right, label='Right') axs[1].grid(b=True, which='both', axis='both') axs[1].legend() t = 0 scatter_pts_x = [] scatter_pts_y = [] scatter_pts_x_1 = [] scatter_pts_y_1 = [] while t <= self.walker.soccerbot.robot_path.duration(): if self.walker.soccerbot.current_step_time <= t <= self.walker.soccerbot.robot_path.duration(): self.walker.soccerbot.stepPath(t, verbose=True) self.walker.soccerbot.apply_imu_feedback(self.walker.soccerbot.get_imu()) sensors = self.walker.soccerbot.get_foot_pressure_sensors(self.walker.ramp.plane) for i in range(len(sensors)): if sensors[i] == True: scatter_pts_x.append(t) scatter_pts_y.append(i) if np.sum(sensors[0:4]) >= 2: scatter_pts_x_1.append(t) scatter_pts_y_1.append(-0.1) if np.sum(sensors[4:8]) >= 2: scatter_pts_x_1.append(t) scatter_pts_y_1.append(0.1) forces = self.walker.soccerbot.apply_foot_pressure_sensor_feedback(self.walker.ramp.plane) pb.setJointMotorControlArray(bodyIndex=self.walker.soccerbot.body, controlMode=pb.POSITION_CONTROL, jointIndices=list(range(0, 20, 1)), targetPositions=self.walker.soccerbot.get_angles(), forces=forces ) self.walker.soccerbot.current_step_time = self.walker.soccerbot.current_step_time + self.walker.soccerbot.robot_path.step_size pb.stepSimulation() t = t + self.walker.PYBULLET_STEP axs[0].scatter(scatter_pts_x, scatter_pts_y, s=3) axs[1].scatter(scatter_pts_x_1, scatter_pts_y_1, s=3) plt.show()
pitch = -10.0 roll = 0 upAxisIndex = 2 camDistance = 4 pixelWidth = 320 pixelHeight = 200 nearPlane = 0.01 farPlane = 100 fov = 60 main_start = time.time() while (1): for yaw in range(0, 360, 10): pybullet.stepSimulation() start = time.time() viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll( camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight projectionMatrix = pybullet.computeProjectionMatrixFOV( fov, aspect, nearPlane, farPlane) img_arr = pybullet.getCameraImage( pixelWidth, pixelHeight, viewMatrix, projectionMatrix, shadow=1, lightDirection=[1, 1, 1], renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
def reset(self, init_joints=None, scene_file=None): """Environment reset""" # Set the camera settings. look = [ -0.35, -0.58, -0.88, ] distance = 1.3 pitch = -41.0 yaw = 180 roll = 0 fov = 60.0 + self._cameraRandom * np.random.uniform(-2, 2) self._view_matrix = p.computeViewMatrixFromYawPitchRoll( look, distance, yaw, pitch, roll, 2) aspect = float(self._window_width) / self._window_height self.near = 0.1 self.far = 10 self._proj_matrix = p.computeProjectionMatrixFOV( fov, aspect, self.near, self.far) # Set table and plane p.resetSimulation() p.setTimeStep(self._timeStep) # Intialize robot and objects p.setGravity(0, 0, -9.81) p.stepSimulation() if init_joints is None: self._panda = Panda(stepsize=self._timeStep, base_shift=self._shift) else: for _ in range(1000): p.stepSimulation() self._panda = Panda(stepsize=self._timeStep, init_joints=init_joints, base_shift=self._shift) plane_file = "data/objects/floor" table_file = "data/objects/table/models" self.plane_id = p.loadURDF( os.path.join(plane_file, 'model_normalized.urdf'), [0 - self._shift[0], 0 - self._shift[1], -0.82 - self._shift[2]]) self.table_id = p.loadURDF( os.path.join(table_file, 'model_normalized.urdf'), 0.5 - self._shift[0], 0.0 - self._shift[1], -0.82 - self._shift[2], 0.707, 0.0, 0.0, 0.707, ) if not self._object_cached: self._objectUids = self.cache_objects() self.obj_path += [plane_file, table_file] self._objectUids += [self.plane_id, self.table_id] self._env_step = 0 return self._get_observation()
def _randomly_place_objects(self, urdfList, scale=1, poses=None): """ Randomize positions of each object urdf. """ xpos = 0.6 + 0.2 * (self._blockRandom * random.random() - 0.5) - self._shift[0] ypos = 0.5 * self._blockRandom * (random.random() - 0.5) - self._shift[0] orn = p.getQuaternionFromEuler([0, 0, 0]) # p.resetBasePositionAndOrientation( self._objectUids[self.target_idx], [xpos, ypos, -0.44 - self._shift[2]], [orn[0], orn[1], orn[2], orn[3]], ) p.resetBaseVelocity(self._objectUids[self.target_idx], (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)) self.cached_objects[self.obj_path.index(urdfList[0])] = True for _ in range(3000): p.stepSimulation() pos, _ = p.getLinkState(self._panda.pandaUid, self._panda.pandaEndEffectorIndex)[:2] pos = np.array([[pos[0], pos[1]], [xpos, ypos]]) k = 0 max_cnt = 50 obj_radius = [ 0.05, np.max(self.cache_object_extents[self.obj_path.index(urdfList[0])]) / 2, ] assigned_orn = [orn] placed_indexes = [self.target_idx] for i, name in enumerate(urdfList[1:]): obj_idx = self.obj_path.index(name) radius = np.max(self.cache_object_extents[obj_idx]) / 2 cnt = 0 if self.cached_objects[obj_idx]: continue while cnt < max_cnt: cnt += 1 xpos_ = xpos - self._blockRandom * 1.0 * random.random() ypos_ = ypos - self._blockRandom * 3 * (random.random() - 0.5 ) # 0.5 xy = np.array([[xpos_, ypos_]]) if (self._check_safe_distance(xy, pos, obj_radius, radius) and (xpos_ > 0.35 - self._shift[0] and xpos_ < 0.65 - self._shift[0]) and (ypos_ < 0.20 - self._shift[1] and ypos_ > -0.20 - self._shift[1])): # 0.15 break if cnt == max_cnt: continue # check 1 xpos_ = xpos_ + 0.05 # closer and closer to the target angle = np.random.uniform(-np.pi, np.pi) orn = p.getQuaternionFromEuler([0, 0, angle]) p.resetBasePositionAndOrientation( self._objectUids[obj_idx], [xpos, ypos_, -0.44 - self._shift[2]], [orn[0], orn[1], orn[2], orn[3]], ) # xyzw p.resetBaseVelocity(self._objectUids[obj_idx], (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)) for _ in range(3000): p.stepSimulation() _, new_orn = p.getBasePositionAndOrientation( self._objectUids[obj_idx]) ang = (np.arccos( 2 * np.power(np.dot(tf_quat(orn), tf_quat(new_orn)), 2) - 1) * 180.0 / np.pi) if ang > 20: p.resetBasePositionAndOrientation( self._objectUids[obj_idx], [xpos, ypos, -10.0 - self._shift[2]], [orn[0], orn[1], orn[2], orn[3]], ) continue # check 2 self.cached_objects[obj_idx] = True obj_radius.append(radius) pos = np.concatenate([pos, xy], axis=0) xpos = xpos_ placed_indexes.append(obj_idx) assigned_orn.append(orn) for _ in range(10000): p.stepSimulation() return True
def main(): # Open GUI and set pybullet_data in the path p.connect(p.GUI) #p.setAdditionalSearchPath(pybullet_data.getDataPath()) # Load plane contained in pybullet_data p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf")) # Set gravity for simulation p.setGravity(0, 0, -9.8) # load robot model robotIds = p.loadSDF( os.path.join(robot_data.getDataPath(), "iCub/icub_fixed_model.sdf")) icubId = robotIds[0] # set constraint between base_link and world p.createConstraint(icubId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [ p.getBasePositionAndOrientation(icubId)[0][0], p.getBasePositionAndOrientation(icubId)[0][1], p.getBasePositionAndOrientation(icubId)[0][2] * 1.2 ], p.getBasePositionAndOrientation(icubId)[1]) ##init_pos for standing # without FT_sensors init_pos = [0] * 15 + [ -29.4, 28.8, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 44.59, 0, 0, 0 ] # with FT_sensors #init_pos = [0]*19 + [-29.4, 28.8, 0, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 0, 44.59, 0, 0, 0] # all set to zero #init_pos = [0]*p.getNumJoints(icubId) # Load other objects p.loadURDF(os.path.join(pybullet_data.getDataPath(), "table/table.urdf"), [1.1, 0.0, 0.0]) p.loadURDF(os.path.join(pybullet_data.getDataPath(), "lego/lego.urdf"), [0.5, 0.0, 0.8]) # add debug slider jointIds = [] paramIds = [] joints_num = p.getNumJoints(icubId) print("len init_pos ", len(init_pos)) print("len num joints", joints_num) for j in range(joints_num): info = p.getJointInfo(icubId, j) jointName = info[1] #jointType = info[2] jointIds.append(j) paramIds.append( p.addUserDebugParameter(jointName.decode("utf-8"), info[8], info[9], init_pos[j] / 180 * m.pi)) while True: for i in range(joints_num): p.setJointMotorControl2(icubId, i, p.POSITION_CONTROL, targetPosition=p.readUserDebugParameter(i), targetVelocity=0.0, positionGain=0.25, velocityGain=0.75, force=50) p.stepSimulation() time.sleep(0.01)
def run_simulation_null_y_COM(dt, t_stance, duration, f, A_lat, th0_lat, th0_abd, thf_abd, z_rotation, girdle_friction, body_friction, last_link_friction, leg_friction, K_lateral, k0_lateral, Kp_lat, Kp_r_abd, Kp_l_abd, Kp_flex, Kv_lat, Kv_r_abd, Kv_l_abd, Kv_flex, keep_in_check=True, graphic_mode=False, plot_graph_joint=False, plot_graph_COM=False, video_logging=False, video_path="./video/trash/trash.mp4"): """ Parameters ---------- dt : float length of simulation time-step (seconds, tipically something between 1/240 and 1/480); values too high can cause instability in the controller due to discretization\n t_stance : float duration of the stance phase (50% symmetric duty cycle)\n duration : float total duration of the simulation\n f : float walking frequency, titpically should be set to 1/(2*t_stance)\n A_lat : float amplitude of the traveling wave used for setting the starting position\n th0_lat : float offset of the traveling wave used for setting the starting position\n th0_abd : float starting value for the leg abduction trajectory\n thf_abd : float ending value for the leg abduction trajectory\n z_rotation : float initial z-rotation of the model reference frame\n girdle_friction : float value of the friction (w/ the floor) of the girdle link\n body_friction : float value of the friction (w/ the floor) of the spinal segments\n last_link_friction : [type] value of the friction (w/ the floor) of the last segment of the spine\n leg_friction : float value of the friction (w/ the floor) of the legs link (feet's friction, substantially)\n K_lateral : float gain of the closed-loop inverse kinematics\n k0_lateral : float k0 value of the closed-loop inverse kinematics, check crawler.py for a better description\n Kp_lat : float spinal lateral joints' proportional gain for the computed torque control\n Kp_r_abd : float right abduction joint's proportional gain for the computed torque control\n Kp_l_abd : float left abduction joint's proportional gain for the computed torque control\n Kp_flex : float leg flexion joints' proportional gain for the computed torque control\n Kv_lat : float spinal lateral joints' derivative gain for the computed torque control\n Kv_r_abd : float right abduction joint's derivative gain for the computed torque control\n Kv_l_abd : float left abduction joint's derivative gain for the computed torque control\n Kv_flex : float leg flexion joints' derivative gain for the computed torque control\n keep_in_check : bool, optional check if the simulation have an unexpected unrealistical behaviour (should be properly set up inside this function definition), by default True\n graphic_mode : bool, optional show GUI of simulation, by default False. Need to be set to True for video logging\n plot_graph_joint : bool, optional plot joints' parameters, by default False\n plot_graph_COM : bool, optional plot COM's trajectory, by default False\n video_logging : bool, optional save a video of the simulation, by default False\n video_path : str, optional filepath for the video file, by default "./video/trash/trash.mp4"\n Returns ------- loss: float\n Value of the loss function of the simulation """ # dt, t_stance, duration = time parameter, dt MUST be the same used to create the model # A,f1,n,t2_off,delta_off,bias = parameters for the generation of the torques # theta_rg0, theta_lg0, A_lat_0, theta_lat_0= parameters for setting the starting position # girdle_friction, body_friction, leg_friction = lateral friction values ########################################## ####### PYBULLET SET-UP ################## if graphic_mode: physicsClient = p.connect(p.GUI) else: physicsClient = p.connect(p.DIRECT) p.setTimeStep(dt) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -9.81) ### plane set-up planeID = p.loadURDF("plane100.urdf", [0, 0, 0]) p.changeDynamics( planeID, linkIndex=-1, lateralFriction=1, spinningFriction=1, rollingFriction=1, #restitution = 0.5, #contactStiffness = 0, #contactDamping = 0 ) ########################################## ####### MODEL SET-UP ##################### model = crw.Crawler( urdf_path="/home/fra/Uni/Tesi/crawler/src", dt_simulation=dt, base_position=[0, 0, 0.05], base_orientation=[0, 0, sin(z_rotation / 2), cos(z_rotation / 2)], mass_distribution=True) # for i,elem in enumerate(model.links_state_array): # print("Link %d "%i, elem["world_com_trn"]) # print(model.links_state_array) # Set motion damping ("air friction") for index in range(-1, model.num_joints): p.changeDynamics(model.Id, index, linearDamping=0.0001, angularDamping=0.0001) # Girdle link dynamic properties p.changeDynamics( model.Id, linkIndex=-1, lateralFriction=girdle_friction, spinningFriction=girdle_friction / 100, rollingFriction=girdle_friction / 100, restitution=0.1, #contactStiffness = 0, #contactDamping = 0 ) # Body dynamic properties for i in range(1, model.num_joints - 4, 2): p.changeDynamics( model.Id, linkIndex=i, lateralFriction=body_friction, spinningFriction=body_friction / 100, rollingFriction=body_friction / 100, restitution=0.1, #contactStiffness = 0, #contactDamping = 0 ) # # Last link dynamic properties p.changeDynamics( model.Id, linkIndex=(model.control_indices[0][-1] + 1), lateralFriction=last_link_friction, spinningFriction=last_link_friction / 100000, rollingFriction=last_link_friction / 100000, restitution=0.1, #contactStiffness = 0, #contactDamping = 0 ) # Legs dynamic properties for i in range(model.num_joints - 3, model.num_joints, 2): p.changeDynamics( model.Id, linkIndex=i, lateralFriction=leg_friction, spinningFriction=leg_friction / 100, rollingFriction=leg_friction / 100, restitution=0.1, #contactStiffness = 0, #contactDamping = 0 ) # Dorsal joints "compliance" model.turn_off_crawler() for i in range(1, model.control_indices[0][-1] + 1, 2): p.setJointMotorControl2(model.Id, i, controlMode=p.VELOCITY_CONTROL, targetVelocity=0, force=0.05) # Starting position model.set_bent_position(theta_rg=th0_abd, theta_lg=-thf_abd, A_lat=A_lat, theta_lat_0=th0_lat) ########################################## ####### CONTROLLER PARAMETERS ############ Kp = model.generate_Kp(Kp_lat, Kp_r_abd, Kp_l_abd, Kp_flex) Kv = model.generate_Kv(Kv_lat, Kv_r_abd, Kv_l_abd, Kv_flex) model.set_low_pass_lateral_qa(fc=10) model.set_low_pass_tau(fc=90) ########################################## ####### MISCELLANEOUS #################### if graphic_mode: p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.resetDebugVisualizerCamera(cameraDistance=0.8, cameraYaw=50, cameraPitch=-60, cameraTargetPosition=[0, 0, 0]) if (video_logging and graphic_mode): video_writer = imageio.get_writer(video_path, fps=int(1 / dt)) ########################################## ####### PERFORMANCE EVALUATION ########### loss = 0.0 max_loss = 10000000.0 # value to set when something goes wrong, if constraints are "hard" max_COM_height = 4 * model.body_sphere_radius max_COM_speed = 10 #TODO: check this value n_mean = 6 ########################################## ####### TIME SERIES CREATION ############# steps = int(duration / dt) steps_stance = int(t_stance / dt) num_lat = len(model.control_indices[0]) f_walk = 1 / (2 * t_stance) # Data array initialization p_COM_time_array = np.zeros((steps, 3)) v_COM_time_array = np.zeros((steps, 3)) tau_time_array = np.zeros((steps, model.state.nqd)) joint_power_time_array = np.zeros((steps, model.state.nqd)) q_time_array = np.zeros((steps, model.state.nq)) qd_time_array = np.zeros((steps, model.state.nqd)) qdd_time_array = np.zeros((steps, model.state.nqd)) eq_time_array = np.zeros((steps, model.state.nq)) q_des_time_array = np.zeros((steps, model.state.nq)) qd_des_time_array = np.zeros((steps, model.state.nqd)) qdd_des_time_array = np.zeros((steps, model.state.nqd)) # Integrator for the work done by each joint joint_power_integrator = crw.Integrator_Forward_Euler( dt, np.zeros(model.state.nqd)) joint_energy_time_array = np.zeros((steps, model.state.nqd)) ########################################## ####### RUN SIMULATION ################### t = 0.0 # let the legs fall and leave the time to check the starting position for i in range(100): p.stepSimulation() if graphic_mode: time.sleep(dt) # Initial Condition model.state.update() model.integrator_lateral_qa.reset(model.state.q[model.mask_act_shifted]) model.free_right_foot() model.fix_left_foot() model.fix_tail(second_last=False) model.set_COM_y_ref() qda_des_prev = np.array(([0] * len(model.control_indices[0]))) # walk and record data for i in range(steps): # UPDATE CONSTRAINTS if not (i % steps_stance): model.invert_feet() print("step") model.turn_off_crawler() for tmp in range(1): p.stepSimulation() # UPDATE STATE model.state.update() p_COM_curr = np.array(model.COM_position_world()) v_COM_curr = np.array(model.COM_velocity_world()) # ADD FRAME TO VIDEO if (graphic_mode and video_logging): img = p.getCameraImage(800, 640, renderer=p.ER_BULLET_HARDWARE_OPENGL)[2] video_writer.append_data(img) # ACTUATE MODEL AND STEP SIMULATION qa_des, qda_des, qdda_des = model.solve_null_COM_y_speed_optimization_qdda( qda_prev=qda_des_prev, K=K_lateral, k0=k0_lateral, filtered=True) qda_des_prev = qda_des q_des, qd_des, qdd_des, eq = model.generate_joints_trajectory( theta0=th0_abd, thetaf=thf_abd, ti=t, t_stance=t_stance, qa_des=qa_des, qda_des=qda_des, qdda_des=qdda_des, cos_abd=True) tau_des, eq = model.solve_computed_torque_control( q_des=q_des, qd_des=qd_des, qdd_des=qdd_des, Kp=Kp, Kv=Kv, #rho=rho, verbose=False) tau_applied = model.apply_torques(tau_des=tau_des, filtered=True) # UPDATE TIME-ARRAYS if plot_graph_joint or plot_graph_COM: q_time_array[i] = model.state.q.copy() qd_time_array[i] = model.state.qd.copy() qdd_time_array[i] = model.state.qdd.copy() q_des_time_array[i] = q_des.copy() qd_des_time_array[i] = qd_des.copy() qdd_des_time_array[i] = qdd_des.copy() p_COM_time_array[i] = p_COM_curr v_COM_time_array[i] = v_COM_curr eq_time_array[i] = q_des_time_array[i] - q_time_array[i] tau_time_array[i] = tau_applied for j, (v_j, tau_j) in enumerate(zip(model.state.qd, tau_applied)): joint_power_time_array[i, j] = abs(v_j * tau_j) joint_energy = joint_power_integrator.integrate( joint_power_time_array[i]) joint_energy_time_array[i] = joint_energy # print("eq: ", np.round(eq[model.mask_act],2), np.round(eq[model.mask_both_legs],2)) # print("tau: ", np.round(tau_applied[model.mask_act],2), np.round(tau_applied[model.mask_both_legs],2)) # print("qdd_des: ", np.round(qdd_des_time_array[i][model.mask_act],2), np.round(qdd_des_time_array[i][model.mask_both_legs],2)) #UPDATE LOSS - VARIATIONAL VERSION -d(x-displacement) + d(total energy expenditure) # loss += ( # -100*(p_COM_time_array[i][0]-p_COM_time_array[i-1][0]) + # 0.1*(joint_power_time_array[i]).dot(joint_power_time_array[i]) # ) # STEP SIMULATION p.stepSimulation() if graphic_mode: time.sleep(dt) t += dt # CHECK SIMULATION to avoid that the model get schwifty if keep_in_check: # check COM height, model shouldn't fly or go through the ground if (p_COM_curr[-1] > (max_COM_height)) or (p_COM_curr[-1] < 0.0): print("\nLook at me I'm jumping\n") loss += max_loss break if (np.linalg.norm(np.mean(v_COM_time_array[i - n_mean:i + 1], 0)) > max_COM_speed): print("\nFaster than light. WOOOOOOOSH\n") loss += max_loss break p.disconnect() ####### END SIMULATION ################### if (video_logging and graphic_mode): video_writer.close() ########################################## ####### PLOT DATA ######################## if plot_graph_joint: fig_lat, axs_lat = plt.subplots(2, 3) for i in range(0, num_lat): #axs_lat[i//3,i%3].plot(qd_time_array[:,model.mask_act][:,i], color="xkcd:dark yellow", label="qd") #axs_lat[i//3,i%3].plot(qdd_time_array[:,model.mask_act][:,i], color="xkcd:pale yellow", label="qdd") #axs_lat[i//3,i%3].plot(joint_power_time_array[:,model.mask_act][:,i], color="xkcd:light salmon", label="Power") #axs_lat[i//3,i%3].plot(joint_energy_time_array[:,model.mask_act][:,i], color="xkcd:dark salmon", label="Energy") axs_lat[i // 3, i % 3].plot(tau_time_array[:, model.mask_act][:, i], color="xkcd:salmon", label="Torque") axs_lat[i // 3, i % 3].plot(q_time_array[:, model.mask_act_shifted][:, i], color="xkcd:yellow", label="q") axs_lat[i // 3, i % 3].plot(q_des_time_array[:, model.mask_act_shifted][:, i], color="xkcd:dark teal", label="q_des") #axs_lat[i//3,i%3].plot(eq_time_array[:,model.mask_act_shifted][:,i], color="xkcd:red", label="error") axs_lat[i // 3, i % 3].set_title("Lateral joint %d" % i) handles_lat, labels_lat = axs_lat[0, 0].get_legend_handles_labels() fig_lat.legend(handles_lat, labels_lat, loc='center right') # fig_girdle, axs_girdle = plt.subplots(2, 2) for i in range(0, len(model.mask_both_legs)): #axs_girdle[i//2,i%2].plot(qd_time_array[:,model.mask_both_legs][:,i], color="xkcd:dark yellow", label="qd") #axs_girdle[i//2,i%2].plot(qdd_time_array[:,model.mask_both_legs][:,i], color="xkcd:pale yellow", label="qdd") #axs_girdle[i//2,i%2].plot(joint_power_time_array[:,model.mask_both_legs][:,i], color="xkcd:light salmon", label="Power") #axs_girdle[i//2,i%2].plot(joint_energy_time_array[:,model.mask_both_legs][:,i], color="xkcd:dark salmon", label="Energy") axs_girdle[i // 2, i % 2].plot(tau_time_array[:, model.mask_both_legs][:, i], color="xkcd:salmon", label="Torque") axs_girdle[i // 2, i % 2].plot( q_time_array[:, model.mask_both_legs_shifted][:, i], color="xkcd:yellow", label="q") axs_girdle[i // 2, i % 2].plot( q_des_time_array[:, model.mask_both_legs_shifted][:, i], color="xkcd:dark teal", label="q_des") axs_girdle[i // 2, i % 2].plot( eq_time_array[:, model.mask_both_legs_shifted][:, i], color="xkcd:red", label="error") axs_girdle[0, 0].set_title("Right abduction") axs_girdle[0, 1].set_title("Right flexion") axs_girdle[1, 0].set_title("Left abduction") axs_girdle[1, 1].set_title("Left flexion") handles_girdle, labels_girdle = axs_girdle[ 0, 0].get_legend_handles_labels() fig_girdle.legend(handles_girdle, labels_girdle, loc='center right') # plt.show() # if plot_graph_COM: fig_COM = plt.figure() axs_COM = fig_COM.add_subplot(111) axs_COM.plot(p_COM_time_array[:, 0], p_COM_time_array[:, 1], color="xkcd:teal", linewidth=4) axs_COM.set_title("COM x-y trajectory") axs_COM.set_xlim(-0.22, 0.01) axs_COM.set_ylim(-0.16, 0.01) axs_COM.set_aspect('equal') # fig_COM_3D = plt.figure() # axs_COM_3D = fig_COM_3D.add_subplot(111, projection='3d') # axs_COM_3D.plot(p_COM_time_array[:,0], p_COM_time_array[:,1], p_COM_time_array[:,2],color="xkcd:teal") # axs_COM_3D.set_title("COM 3D trajectory") # plt.show() return loss
def update_physics(delay, quadcopterId, quadcopter_controller, config, graph: datalogger): while quadcopter_controller.sim: #start = time.perf_counter() #start = time.clock() # the controllers are evaluated at a slower rate, only once in the # control_subsample times the controller is evaluated # if quadcopter_controller.sample == 0: # quadcopter_controller.sample = control_subsample # pos_meas,quaternion_meas = p.getBasePositionAndOrientation(quadcopterId) # force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas) # else: # quadcopter_controller.sample -= 1 pos_meas, quaternion_meas = p.getBasePositionAndOrientation( quadcopterId) rotmat = quaternion2rotation_matrix(quaternion_meas) front = np.array([pos_meas[0] + 0.1750, pos_meas[1], pos_meas[2]]) back = np.array([pos_meas[0] - 0.1750, pos_meas[1], pos_meas[2]]) left = np.array([pos_meas[0], pos_meas[1] + 0.1750, pos_meas[2]]) right = np.array([pos_meas[0], pos_meas[1] - 0.1750, pos_meas[2]]) front_meas = np.dot(rotmat.T, front.reshape(3, 1)).reshape(3) back_meas = np.dot(rotmat.T, back.reshape(3, 1)).reshape(3) left_meas = np.dot(rotmat.T, left.reshape(3, 1)).reshape(3) right_meas = np.dot(rotmat.T, right.reshape(3, 1)).reshape(3) force_act1, force_act2, force_act3, force_act4, moment_yaw = quadcopter_controller.update_control( pos_meas, quaternion_meas, config) radius = 0.25 ige_force1 = ige_force2 = ige_force3 = ige_force4 = 1.0 if (front_meas[2] < 5 * radius): ige_force1 = groundEffect.groundEffect(front_meas[2], radius) if (back_meas[2] < 5 * radius): ige_force3 = groundEffect.groundEffect(back_meas[2], radius) if (left_meas[2] < 5 * radius): ige_force2 = groundEffect.groundEffect(left_meas[2], radius) if (back_meas[2] < 5 * radius): ige_force4 = groundEffect.groundEffect(right_meas[2], radius) if graph.capture: graph.addpoint(pos_meas[2], force_act1[2]) force_act1 = force_act1 / ige_force1 force_act2 = force_act2 / ige_force2 force_act3 = force_act3 / ige_force3 force_act4 = force_act4 / ige_force4 #capturing forces with altitude to plot # apply forces/moments from controls etc: # (do this each time, because forces and moments are reset to zero after a stepSimulation()) p.applyExternalForce(quadcopterId, -1, force_act1, [config.arm_length, 0., 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act2, [0., config.arm_length, 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act3, [-config.arm_length, 0., 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act4, [0., -config.arm_length, 0.], p.LINK_FRAME) # for the yaw-control: p.applyExternalTorque(quadcopterId, -1, [0., 0., moment_yaw], p.LINK_FRAME) # do simulation with pybullet: p.stepSimulation()
def run(): physicsClient = p.connect( p.GUI) # p.GUI for graphical, or p.DIRECT for non-graphical version p.setAdditionalSearchPath( pybullet_data.getDataPath()) # defines the path used by p.loadURDF p.setGravity(0, 0, -10) p.setPhysicsEngineParameter(enableConeFriction=1) p.setRealTimeSimulation( 0 ) # only if this is set to 0 and with explicit steps will the torque control work correctly # load the ground plane planeId = p.loadURDF("plane.urdf") # add a box for blocked force boxStartPos = [0, -1, 1.4] boxStartOr = p.getQuaternionFromEuler([0, 0, 0]) boxId, boxConstraintId = load_constrained_urdf( "urdfs/smallSquareBox.urdf", boxStartPos, boxStartOr, physicsClient=physicsClient, ) p.changeDynamics(boxId, -1, lateralFriction=2) # load finger finger = SMContinuumManipulator(manipulator_definition) finger_startPos = [0, 1, 2.05] finger_StartOr = p.getQuaternionFromEuler([-np.pi / 2, 0, np.pi]) finger.load_to_pybullet( baseStartPos=finger_startPos, baseStartOrn=finger_StartOr, baseConstraint="static", physicsClient=physicsClient, ) p.changeDynamics(finger.bodyUniqueId, -1, lateralFriction=2) p.changeDynamics(finger.bodyUniqueId, -1, restitution=1) time_step = 0.001 p.setTimeStep(time_step) n_steps = 20000 sim_time = 0.0 lam = 1.0 omega = 1.0 torque_fns = [ lambda t: 20 * np.sin(omega * t), lambda t: 20 * np.sin(omega * t - 1 * np.pi), ] # - 0pi goes right, - pi goes left real_time = time.time() normal_forces = np.zeros((n_steps, )) normal_forces_lastLink = np.zeros((n_steps, )) time_plot = np.zeros((n_steps, )) last_link = p.getNumJoints( finger.bodyUniqueId) - 1 # last link of the finger # wait for screen recording for demo wait_for_rec = False if wait_for_rec: time.sleep(5) print("6 more seconds") time.sleep(6) for i in range(n_steps): print(torque_fns[0](sim_time)) finger.apply_actuationTorque(actuator_nr=0, axis_nr=0, actuation_torques=torque_fns[0](sim_time)) finger.apply_actuationTorque(actuator_nr=0, axis_nr=1, actuation_torques=0) p.stepSimulation() sim_time += time_step # time it took to simulate delta = time.time() - real_time real_time = time.time() # print(delta) contactPoints = p.getContactPoints(boxId, finger.bodyUniqueId, physicsClientId=physicsClient) contactPointsTip = p.getContactPoints( bodyA=boxId, bodyB=finger.bodyUniqueId, linkIndexB=last_link, physicsClientId=physicsClient, ) # print(" ---------- normal force ---------") time_plot[i] = sim_time total_normal_force = 0 for contactPoint in contactPoints: normal_force = contactPoint[9] total_normal_force += normal_force total_normal_force_lastLink = 0 print(len(contactPoints), len(contactPointsTip)) for contactPointTip in contactPointsTip: normal_forceTip = contactPointTip[9] print(normal_forceTip) total_normal_force_lastLink += normal_forceTip normal_forces[i] = total_normal_force normal_forces_lastLink[i] = total_normal_force_lastLink p.disconnect() plt.plot(time_plot, normal_forces) plt.xlabel("time") plt.ylabel("total normal force") plt.show() plt.plot(time_plot, normal_forces_lastLink) plt.xlabel("time") plt.ylabel("total normal force actuator tip") plt.show()
pitch = -10.0 roll=0 upAxisIndex = 2 camDistance = 4 pixelWidth = 320 pixelHeight = 200 nearPlane = 0.01 farPlane = 100 fov = 60 main_start = time.time() while(1): for yaw in range (0,360,10) : pybullet.stepSimulation() start = time.time() viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) stop = time.time() print ("renderImage %f" % (stop - start)) w=img_arr[0] #width of the image, in pixels h=img_arr[1] #height of the image, in pixels rgb=img_arr[2] #color data RGB dep=img_arr[3] #depth data #print(rgb) print ('width = %d height = %d' % (w,h))
def step(self): p.stepSimulation() for o in self.binds: o.update()
f = [aabbMax[0], aabbMax[1], aabbMax[2]] t = [aabbMax[0], aabbMin[1], aabbMax[2]] p.addUserDebugLine(f, t, [1, 1, 1]) f = [aabbMax[0], aabbMax[1], aabbMax[2]] t = [aabbMax[0], aabbMax[1], aabbMin[2]] p.addUserDebugLine(f, t, [1, 1, 1]) aabb = p.getAABB(r2d2) aabbMin = aabb[0] aabbMax = aabb[1] if (printtext): print(aabbMin) print(aabbMax) if (draw == 1): drawAABB(aabb) for i in range(p.getNumJoints(r2d2)): aabb = p.getAABB(r2d2, i) aabbMin = aabb[0] aabbMax = aabb[1] if (printtext): print(aabbMin) print(aabbMax) if (draw == 1): drawAABB(aabb) while (1): a = 0 p.stepSimulation()
#### Apply z-axis torque to the base ############################################################### # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) #### To propeller 0 ################################################################################ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) # p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) #### To the center of mass ######################################################################### # p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0., 0., 5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) #### Draw base frame ############################################################################### env._showDroneFrame(0) #### Step, sync the simulation, printout the state ################################################# p.stepSimulation(physicsClientId=PYB_CLIENT) sync(i, START, env.TIMESTEP) if i % env.SIM_FREQ == 0: env.render() #### Reset ######################################################################################### if i > 1 and i % ((DURATION_SEC / (NUM_RESETS + 1)) * env.SIM_FREQ) == 0: env.reset() p.setGravity(0, 0, 0, physicsClientId=PYB_CLIENT) #### Close the environment ######################################################################### env.close()