def get_current_detailed_config(self): # Get the object states gpos, gquat = p.getBasePositionAndOrientation(self.green_block, physicsClientId=0) rpos, rquat = p.getBasePositionAndOrientation(self.red_block, physicsClientId=0) g_linear_vel, g_angular_velocity = p.getBaseVelocity(self.green_block, physicsClientId=0) r_linear_vel, r_angular_velocity = p.getBaseVelocity(self.red_block, physicsClientId=0) # Convert quat to euler geuler = p.getEulerFromQuaternion(gquat) reuler = p.getEulerFromQuaternion(rquat) # Format into a config vector return { "object_1": np.concatenate( [gpos, gquat, geuler, g_linear_vel, g_angular_velocity]), "object_2": np.concatenate( [rpos, rquat, reuler, r_linear_vel, r_angular_velocity]) }
def recordData(): xList.append(p.getBasePositionAndOrientation(robotId)[0][0]) zList.append(p.getBasePositionAndOrientation(robotId)[0][2]) pitchList.append(p.getBasePositionAndOrientation(robotId)[1][1]) bodyAngleList.append(p.getJointState(robotId, 0)[0]) heightList.append(p.getBasePositionAndOrientation(robotId)[0][2]) xf = (p.getLinkState(robotId, 3)[0][0] - p.getLinkState(robotId, 0)[0][0]) xdList.append(p.readUserDebugParameter(xd)) xfList.append(xf) xfd = (p.getBaseVelocity(robotId)[0][0] * 0.39 / 2 + p.readUserDebugParameter(footFeedbackGain) * (p.getBaseVelocity(robotId)[0][0])) desiredForwardSpeedX = getDesiredSpeed( currentPosition=( p.getBasePositionAndOrientation(robotId)[0][0] ), currentSpeed=p.getBaseVelocity(robotId)[0][0], targetPosition=xd, kp=kPosition, kv=kVelocity, maxForwardSpeed=maxSpeed) xVdList.append(desiredForwardSpeedX) xVelocityList.append(p.getBaseVelocity(robotId)[0][0]) yVelocityList.append(p.getBaseVelocity(robotId)[0][1])
def update(self): pos = np.array(pybullet.getBasePositionAndOrientation(self._demon)[0]) vel = np.array(pybullet.getBaseVelocity(self._demon)[0]) pos = pos + (vel * self._dt) pos[2] = 0.5 # Need to do this so the intersections are computed for i in range(self.sim_steps): for particle in self._particles: pos, ori = [ np.array(_) for _ in pybullet.getBasePositionAndOrientation(particle) ] lin_vel, ang_vel = pybullet.getBaseVelocity(particle) pos[-1] = self._wall_heights[0] pybullet.resetBasePositionAndOrientation(particle, posObj=pos, ornObj=ori) # noise = np.random.normal(loc=0,scale=0.1,size=3) pybullet.resetBaseVelocity(particle, lin_vel, ang_vel) pybullet.stepSimulation() for particle in self._particles: target_base_vel = pybullet.getBaseVelocity(particle)[0] updated_vel = target_base_vel + np.random.normal( loc=0., size=(3, ), scale=0.75) updated_vel[-1] = 0. pybullet.resetBaseVelocity(particle, linearVelocity=updated_vel) reward = self.computeReward(state=None) self.__reward = reward
def getObservation(self): if (self._flat_obs): obs = np.array( self._obs_stack).flatten() / 255.0 ## Normalize to [0,1] else: obs = np.array( self.getlocalMapObservation()) / 255.0 ## Normalize to [0,1] # print ("obs 1: ", obs) # obs = np.array(self.getlocalMapObservation()).flatten() # obs = np.array(self.getlocalMapObservation()).flatten() # print ("obs 2: ", obs) # print ("self._obs_stack: ", self._obs_stack) if (self._include_true_state): pos = np.array( pybullet.getBasePositionAndOrientation(self._demon)[0]) particle = self._get_target() posT = np.array( pybullet.getBasePositionAndOrientation(particle)[0]) goalDirection = posT - pos vel_d = np.array(pybullet.getBaseVelocity(particle)[0]) base_vel = pybullet.getBaseVelocity(self._demon)[0] obs_ = np.array([ pos[0], pos[1], base_vel[0], base_vel[1], goalDirection[0], goalDirection[1], vel_d[0], vel_d[1] ]).flatten() # print ("obs_ :", obs_) obs = [obs_, obs] return obs
def _observe_info(self, visualize=True): info = {} # number of wall contacts info['wc'] = 0 if self.level > 0: for wallId in self.walls: contactInfo = p.getContactPoints(self.blockId, wallId) info['wc'] += len(contactInfo) # block velocity v, va = p.getBaseVelocity(self.blockId) info['bv'] = np.linalg.norm(v) info['bva'] = np.linalg.norm(va) # pusher velocity v, va = p.getBaseVelocity(self.pusherId) info['pv'] = np.linalg.norm(v) info['pva'] = np.linalg.norm(va) # block-pusher distance x, y, _ = self._observe_pusher() xb, yb, theta = self._observe_block() along, from_center = pusher_pos_along_face((xb, yb), theta, (x, y)) info['pusher dist'] = from_center - DIST_FOR_JUST_TOUCHING info['pusher along'] = along self._observe_additional_info(info, visualize) self._sim_step += 1 for key, value in info.items(): if key not in self._contact_info: self._contact_info[key] = [] self._contact_info[key].append(value)
def computeReward(self, state=None): import numpy as np goalDir = self.getTargetDirection() # goalDir = goalDir / np.sqrt((goalDir*goalDir).sum(axis=0)) # print ("goalDir: ", goalDir) agentVel = np.array(p.getBaseVelocity(self._agent)[0]) # agentSpeed = np.sqrt((agentVel*agentVel).sum(axis=0)) velDiff = goalDir - agentVel diffMag = np.sqrt((velDiff*velDiff).sum(axis=0)) # agentVel = agentVel / agentSpeed # print ("agentVel: ", agentVel) # agentSpeedDiff = (1 - agentSpeed) ### heading towards goal # reward = np.dot(goalDir, agentVel) + np.exp(agentSpeedDiff*agentSpeedDiff * -2.0) reward = np.exp((diffMag*diffMag) * -2.0) llc_dir = np.array([self._llc_target[0], self._llc_target[1], 0]) ### normalize # llc_dir = llc_dir / np.sqrt((llc_dir*llc_dir).sum(axis=0)) # relative_llc_goal_state = (llc_dir-self._last_state[:3]) des_change = llc_dir - p.getBaseVelocity(self._agent)[0] # des_change = (self._last_pose + llc_dir) - np.array(p.getBasePositionAndOrientation(self._agent)[0]) # print ("self._last_state[1][:3] - p.getBaseVelocity(self._agent)[0]: ", self._last_state[1][:3] - p.getBaseVelocity(self._agent)[0]) # llc_reward = np.dot(agentDir, llc_dir) - 1 # llc_reward = -(agentDir*llc_dir).sum(axis=0) # llc_reward = np.exp((llc_reward*llc_reward) * -2.0) # print ("des_change: ", des_change*des_change) llc_reward = -(des_change*des_change).sum(axis=0) # print ("llc_reward: ", llc_reward) return llc_reward
def simulate(sp): # Global body params visual_shape_id = -1 use_maximal_coordinates = 0 sim_time = 5 # Create target (i.e. pollen) shape and body target = p.createCollisionShape(p.GEOM_SPHERE, radius=sp['t_rad'], physicsClientId=sp['pci']) tuid = p.createMultiBody(sp['t_m'], target, visual_shape_id, sp['t_pos'], useMaximalCoordinates=use_maximal_coordinates, physicsClientId=sp['pci']) p.changeDynamics(tuid, -1, restitution=1 - 0.0001) # Create projectile (i.e. air) shape and body proj_rad = 0.15 proj_mass = 0.01 proj_pos = [0, 0, 0.9] proj = p.createCollisionShape(p.GEOM_SPHERE, radius=sp['p_rad'], physicsClientId=sp['pci']) puid = p.createMultiBody(sp['p_m'], proj, visual_shape_id, sp['p_pos'], useMaximalCoordinates=use_maximal_coordinates, physicsClientId=sp['pci']) p.changeDynamics(puid, -1, restitution=1 - 0.0001) # Give projectile initial velocity p.resetBaseVelocity(puid, sp['lv'], sp['av'], physicsClientId=sp['pci']) # Start simulation p.setGravity(0, 0, 0, physicsClientId=sp['pci'] ) # ignore gravity since this is orientation-agnostic first = True while first or time.time() - start < sp['sim_time']: p.stepSimulation(physicsClientId=sp['pci']) foo = len(p.getContactPoints(physicsClientId=sp['pci'])) if foo > 0 and first: start = time.time() first = False keys = p.getKeyboardEvents() time.sleep(0.001) print 'bv' + str(sp['pci']) + ': ' + str( p.getBaseVelocity(tuid, physicsClientId=sp['pci'])) return p.getBaseVelocity(tuid, physicsClientId=sp['pci'])
def cost_function(self, tau, des_a, leg_down): time_step = 1/1000.0 # 单步时间长度 rid = self.robot_id state_id = p.saveState() # 保存当前系统状态 md = p.TORQUE_CONTROL # 扭矩控制模式 leg_id_li = [0, 1, 2, 4, 5, 6] for i in range(6): p.setJointMotorControl2(rid, leg_id_li[i], md, tau[i]) # 1. 获取前一时刻状态 # 机体速度和角速度 b_lv, b_av = np.array(p.getBaseVelocity(rid)) # 摆动腿位置 if leg_down == 'left': vel_a = p.getJointState(rid, 4)[1] vel_b = p.getJointState(rid, 5)[1] vel_c = p.getJointState(rid, 6)[1] else: vel_a = p.getJointState(rid, 0)[1] vel_b = p.getJointState(rid, 1)[1] vel_c = p.getJointState(rid, 2)[1] # 2. 进行一步仿真 p.stepSimulation() # 进行一个步长为1/240s的仿真 # 3. 获取仿真一步后的速度等状态 # body加速度 acc_bv = (np.array(p.getBaseVelocity(self.robot_id)[0]) - b_lv)/time_step # 摆动腿加速度 if leg_down == 'left': acc_a = (p.getJointState(rid, 0)[1] - vel_a) / time_step acc_b = (p.getJointState(rid, 1)[1] - vel_b) / time_step acc_c = (p.getJointState(rid, 2)[1] - vel_c) / time_step else: acc_a = (p.getJointState(rid, 4)[1] - vel_a) / time_step acc_b = (p.getJointState(rid, 5)[1] - vel_b) / time_step acc_c = (p.getJointState(rid, 6)[1] - vel_c) / time_step acc_foot = np.array([acc_a, acc_b, acc_c]) # body角加速度 acc_ba = (np.array(p.getBaseVelocity(self.robot_id)[1]) - b_av)/time_step # 4. 与期望加速度计算Cost函数 w_com = 25 w_bod = np.array([20, 60, 14]) w_foot = 1 c_com = np.linalg.norm(w_com * (acc_bv - des_a['com'])) c_bod = np.linalg.norm(w_bod * (acc_ba - des_a['body'])) c_foo = np.linalg.norm(w_foot * (acc_foot - des_a['foot'])) # c_tau= 0.001 * sum(abs(tau)) # 关节扭矩之和 cost = c_com + c_bod + c_foo # 获取地面接触力状态,似乎pybullet没法获取切向力的,只有法向力 #contact_list = p.getContactPoints(rid) # 5. 恢复状态并返回cost p.restoreState(state_id) return cost
def get_variable_state(self, variable_name): """ :param variable_name: (str) variable name to query about the object. :return: (nd.array, float or int) returns the corresponding value of the variable. """ if variable_name == 'type': return self._type_id elif variable_name == 'cartesian_position': position, orientation = pybullet.getBasePositionAndOrientation( self._block_ids[0], physicsClientId=self._pybullet_client_ids[0]) position = np.array(position) position[-1] -= WorldConstants.FLOOR_HEIGHT return position elif variable_name == 'cylindrical_position': position, orientation = pybullet.getBasePositionAndOrientation( self._block_ids[0], physicsClientId=self._pybullet_client_ids[0]) position = np.array(position) position[-1] -= WorldConstants.FLOOR_HEIGHT return cart2cyl(position) elif variable_name == 'orientation': position, orientation = pybullet.getBasePositionAndOrientation( self._block_ids[0], physicsClientId=self._pybullet_client_ids[0]) position = np.array(position) position[-1] -= WorldConstants.FLOOR_HEIGHT return orientation elif variable_name == 'linear_velocity': linear_velocity, angular_velocity = pybullet.getBaseVelocity( self._block_ids[0], physicsClientId=self._pybullet_client_ids[0]) return linear_velocity elif variable_name == 'angular_velocity': linear_velocity, angular_velocity = pybullet.getBaseVelocity( self._block_ids[0], physicsClientId=self._pybullet_client_ids[0]) return angular_velocity elif variable_name == 'mass': return self._mass elif variable_name == 'size': return self._size elif variable_name == 'color': return self._color elif variable_name == 'friction': return self._lateral_friction
def controlBodyAttitude(): kp = p.readUserDebugParameter(balanceKp) kv = p.readUserDebugParameter(balanceKv) # Balance along X axis pitchAngle = p.getBasePositionAndOrientation(robotId)[1][1] pitchAngleDerivative = p.getBaseVelocity(robotId)[1][1] torque = -kp * (pitchAngle - 0) - kv * pitchAngleDerivative sendTorqueControl(0, -torque) # Balance along Y axis rollAngle = p.getBasePositionAndOrientation(robotId)[1][0] rollAngleDerivative = p.getBaseVelocity(robotId)[1][0] torque2 = -kp * (rollAngle - 0) - kv * rollAngleDerivative sendTorqueControl(1, -torque2)
def getObservation(self): observation = [] # get ee pose state = p.getLinkState(bodyUniqueId=self.neobotixschunkUid, linkIndex=self.endEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1) pos = state[0] orn = state[1] if len(state) > 6: vel = state[6] euler = p.getEulerFromQuaternion(orn) observation.extend(list(pos)) observation.extend(list(euler)) # observation.extend(list(vel)) # get base pose basepos, baseorn = p.getBasePositionAndOrientation( self.neobotixschunkUid) baseeul = p.getEulerFromQuaternion(baseorn) observation.extend(list(basepos)) observation.extend(list(baseeul)) # get joint positions # joint_s = p.getJointStates(bodyUniqueId=self.neobotixschunkUid, jointIndices=list(self.armIndex)) for i in self.armIndex: joints = p.getJointState(bodyUniqueId=self.neobotixschunkUid, jointIndex=i) observation.append((joints[0])) # get base linear and angular vel basev = p.getBaseVelocity(self.neobotixschunkUid) observation.extend(list(basev[0][0:2])) observation.append(basev[1][2]) return observation
def getExtendedObservation(self): # get robot observation self._observation = self._icub.getObservation() # get object position and transform it wrt hand c.o.m. frame objPos, objOrn = p.getBasePositionAndOrientation(self._objID) objEuler = p.getEulerFromQuaternion(objOrn) #roll, pitch, yaw objLVel, objAVel = p.getBaseVelocity(self._objID) self._observation.extend(list(objPos)) self._observation.extend(list(objEuler)) self._observation.extend(list(objLVel)) self._observation.extend(list(objAVel)) # relative object position wrt hand c.o.m. frame invHandPos, invHandOrn = p.invertTransform( self._observation[:3], p.getQuaternionFromEuler(self._observation[3:6])) objPosInHand, objOrnInHand = p.multiplyTransforms( invHandPos, invHandOrn, objPos, objOrn) objEulerInHand = p.getEulerFromQuaternion(objOrnInHand) self._observation.extend(list(objPosInHand)) self._observation.extend(list(objEulerInHand)) #relLVel = np.array(objAVel) - np.array(self._observation[6:9]) #self._observation.extend(list(relLVel)) self._observation.extend(list(self._tg_pose)) return np.array(self._observation)
def collect_observations(drone): #print("ordered_joint_indices") #print(ordered_joint_indices) jointStates = p.getJointStates(drone,ordered_joint_indices) j = np.array([current_relative_position(jointStates, drone, *jtuple) for jtuple in ordered_joints]).flatten() #print("j") #print(j) body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(drone) #print("body_xyz") #print(body_xyz, qx,qy,qz,qw) z = body_xyz[2] dummy.distance = body_xyz[0] if dummy.initial_z==None: dummy.initial_z = z (vx, vy, vz), _ = p.getBaseVelocity(drone) more = np.array([z-dummy.initial_z, 0.1*vx, 0.1*vy, 0.1*vz, qx, qy, qz, qw]) # rcont = p.getContactPoints(drone, -1, right_foot, -1) # #print("rcont") # #print(rcont) # lcont = p.getContactPoints(drone, -1, left_foot, -1) # #print("lcont") # #print(lcont) # feet_contact = np.array([len(rcont)>0, len(lcont)>0]) # return np.clip( np.concatenate([more] + [j] + [feet_contact]), -5, +5) return np.clip( np.concatenate([more] + [j]), -5, +5)
def collect_observations(self, human): #print("ordered_joint_indices") #print(ordered_joint_indices) jointStates = p.getJointStates(human,self.ordered_joint_indices) j = np.array([self.current_relative_position(jointStates, human, *jtuple) for jtuple in self.ordered_joints]).flatten() #print("j") #print(j) body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human) #print("body_xyz") #print(body_xyz, qx,qy,qz,qw) z = body_xyz[2] self.distance = body_xyz[0] if self.initial_z==None: self.initial_z = z (vx, vy, vz), _ = p.getBaseVelocity(human) more = np.array([z-self.initial_z, 0.1*vx, 0.1*vy, 0.1*vz, qx, qy, qz, qw]) rcont = p.getContactPoints(human, -1, self.right_foot, -1) #print("rcont") #print(rcont) lcont = p.getContactPoints(human, -1, self.left_foot, -1) #print("lcont") #print(lcont) feet_contact = np.array([len(rcont)>0, len(lcont)>0]) return np.clip( np.concatenate([more] + [j] + [feet_contact]), -5, +5)
def compute_reward(self): robotPos, robotOrn = p.getBasePositionAndOrientation(self.botId) robotEuler = p.getEulerFromQuaternion(robotOrn) linear, angular = p.getBaseVelocity(self.botId) r = sum(linear) - sum(angular) # receive a bonus of 1 for balancing and pay a small cost proportional to speed return r #1.0 - abs(self.vt) * 0.05
def fitness(paramlist, robotID): reward = 0 for ij in range(7200): sleep(1 / 240) r_hip = sine_signal(paramlist[0], paramlist[1], ij, paramlist[2]) r_knee = sine_signal(paramlist[3], paramlist[4], ij, paramlist[5], pi / 2) l_hip = sine_signal(paramlist[0], paramlist[1], ij, paramlist[2], pi) l_knee = sine_signal(paramlist[3], paramlist[4], ij, paramlist[5], 3 * pi / 2) action = [r_hip, r_knee, l_hip, l_knee, l_hip, l_knee, r_hip, r_knee] for joint, action in zip(joints, action): p.setJointMotorControl2(robotID, joint, p.POSITION_CONTROL, action, force=max_torque, maxVelocity=max_ang_vel) p.stepSimulation() lin_vel, _ = p.getBaseVelocity(robotID) _, orientQuaternion = p.getBasePositionAndOrientation(robotID) orientRad = p.getEulerFromQuaternion(orientQuaternion) reward += lin_vel[1] if abs(orientRad[0]) >= 5 or abs(orientRad[1]) >= 5 or abs( orientRad[2]) >= 1.57: reset() return reward reset() return reward
def set_position(object_idx, position): # reuse existing quaternion _, quaternion = pb.getBasePositionAndOrientation(object_idx) # resetBasePositionAndOrientation zeroes out velocities, but we wish to conserve them velocity, angular_velocity = pb.getBaseVelocity(object_idx) pb.resetBasePositionAndOrientation(object_idx, position, quaternion) pb.resetBaseVelocity(object_idx, velocity, angular_velocity)
def get_base_velocity_world(self): """ Returns the velocity of the base in the world frame. Returns: np.array((6,1)) with the translation and angular velocity """ vel, orn = p.getBaseVelocity(self.robot_id) return np.array(vel + orn).reshape(6, 1)
def get_state(self): # Returns a pinocchio like representation of the q, dq matrixes q = zero(self.nq) dq = zero(self.nv) if not self.useFixedBase: pos, orn = p.getBasePositionAndOrientation(self.robot_id) q[:3] = pos q[3:7] = orn vel, orn = p.getBaseVelocity(self.robot_id) dq[:3] = vel dq[3:6] = orn # Pinocchio assumes base velocity to be in body frame -> rotate. rot = np.array(p.getMatrixFromQuaternion(q[3:7])).reshape((3, 3)) dq[0:3] = rot.T.dot(dq[0:3]) dq[3:6] = rot.T.dot(dq[3:6]) # Query the joint readings. joint_states = p.getJointStates(self.robot_id, self.bullet_joint_ids) if not self.useFixedBase: for i in range(self.nj): q[5 + self.pinocchio_joint_ids[i]] = joint_states[i][0] dq[4 + self.pinocchio_joint_ids[i]] = joint_states[i][1] else: for i in range(self.nj): q[self.pinocchio_joint_ids[i] - 1] = joint_states[i][0] dq[self.pinocchio_joint_ids[i] - 1] = joint_states[i][1] return q, dq
def is_static(self): """Checks if env is static, used for checking if action finished. However, this won't work in PyBullet (at least v2.8.4) since soft bodies cause this code to hang. Therefore, look at the task's `def_IDs` list, which by design will have all IDs of soft bodies. Furthermore, for the bag tasks, the beads generally move around, so for those, just use a hard cutoff limit (outside this method). """ if self.is_softbody_env(): assert len(self.task.def_IDs) > 0, 'Did we forget to add to def_IDs?' v = [np.linalg.norm(p.getBaseVelocity(i)[0]) for i in self.objects if i not in self.task.def_IDs] else: v = [np.linalg.norm(p.getBaseVelocity(i)[0]) for i in self.objects] return all(np.array(v) < 1e-2)
def imu_state(id=0): if SIMULATION == True: _, quat = p.getBasePositionAndOrientation(bipedId) _, v_angular = p.getBaseVelocity(bipedId) return R.from_quat(quat), np.array(v_angular) else: return np.array([]), np.array([])
def is_static(self): """Return true if objects are no longer moving.""" v = [ np.linalg.norm(p.getBaseVelocity(i)[0]) for i in self.obj_ids['rigid'] ] return all(np.array(v) < 5e-3)
def compute_observation(self): pos, orn = p.getBasePositionAndOrientation(self.copter) linear, _ = p.getBaseVelocity(self.copter) norm_linear = [l / 7 for l in linear] norm_pos = [pos[0] / 4, pos[1] / 4, pos[2] / 4 - 1] observation = [*linear, *pos, *orn] return observation
def _compute_reward(self): quadruped_pos, quadruped_orientation = p.getBasePositionAndOrientation( self.quadruped_id) quadruped_linear_vel, quadruped_angular_vel = p.getBaseVelocity( self.quadruped_id) joint_torques = [] for j in self.quadruped_joint_ids: joint_torques.append(p.getJointState(self.quadruped_id, j)[3]) vel_x = quadruped_linear_vel[0] vel_y = quadruped_linear_vel[1] vel_yaw = quadruped_angular_vel[2] quadruped_orientation = p.getEulerFromQuaternion(quadruped_orientation) if vel_x < self.target_velocity: rew_vel_x = vel_x else: rew_vel_x = (2 * self.target_velocity) - vel_x reward = 1 * rew_vel_x - 0.01 * np.abs(vel_y) \ - 0.01 * np.abs(vel_yaw) \ - 0.01 * np.abs(quadruped_orientation[0]) - 0.01 * np.abs(quadruped_orientation[1]) \ - 0.0001 * np.linalg.norm(self.prev_torques - joint_torques) self.prev_torques = np.asarray(joint_torques) return reward
def get_obs(self): # Base position and orientation torso_pos, torso_orient = p.getBasePositionAndOrientation(self.robotId) # Target position and orientation target_pos, _ = p.getBasePositionAndOrientation(self.targetId) # Base velocity and angular velocity torso_pos_, torso_orient_ = p.getBaseVelocity(self.robotId) # Joint states and velocities q, q_, _, _ = zip(*p.getJointStates(self.robotId, self.joint_ids)) obs_dict = { 'torso_pos': torso_pos, 'torso_quat': torso_orient, 'target_pos': target_pos[0:2], 'q': q, 'torso_vel': torso_pos_, 'torso_angvel': torso_orient_, 'q_vel': q_ } obs_arr = np.concatenate([v for v in obs_dict.values() ]).astype(np.float32) return obs_arr, obs_dict
def computeReward(self, state=None): pos = np.array(pybullet.getBasePositionAndOrientation(self._demon)[0]) particle = self._get_target() posT = np.array(pybullet.getBasePositionAndOrientation(particle)[0]) goalDirection = posT - pos goalDistance = np.sqrt((goalDirection * goalDirection).sum(axis=0)) goalDir = self.getTargetDirection() agentVel = np.array(pybullet.getBaseVelocity(self._demon)[0]) velDiff = goalDir - agentVel diffMag = np.sqrt((velDiff * velDiff).sum(axis=0)) # heading towards goal reward = np.exp((diffMag * diffMag) * -2.0) + np.exp( (goalDistance * goalDistance) * -2.0) """ if (goalDistance < 1.5): # Reached goal reward = reward + self._map_width """ # Check contacts with obstacles """ for box_id in self._blocks: contacts = p.getContactPoints(self._agent, box_id) # print ("contacts: ", contacts) if len(contacts) > 0: reward = reward + -self._map_width break """ return reward
def get_state(self): # state of the center of column center_position, center_angle = p.getBasePositionAndOrientation( self.unicycleUid) center_velocity, center_angular_velocity = p.getBaseVelocity( self.unicycleUid) # state of joint points (only get angular velocities of links) link_states = [] for i in self.jointinfo: link_states.append( p.getLinkState(self.unicycleUid, linkIndex=i, computeLinkVelocity=1)[7]) state = np.concatenate( (center_position, center_angle, center_velocity, center_angular_velocity, link_states[0], link_states[1]), axis=None) # fall down or out of ground ends this episode out_of_ground = (np.abs(state[0]) > self.floor_r) or (np.abs(state[1]) > self.floor_r) fall_down = state[2] < self.down or state[2] > 1.5 done = fall_down or out_of_ground reward = 1 if done: # Penalty reward = 0 if self.get_image: return self.img_from_state(state.flatten()), done, reward else: return state.flatten(), done, reward
def collect_observations(self, human): #print("ordered_joint_indices") #print(ordered_joint_indices) jointStates = p.getJointStates(human, self.ordered_joint_indices) j = np.array([ self.current_relative_position(jointStates, human, *jtuple) for jtuple in self.ordered_joints ]).flatten() #print("j") #print(j) body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human) #print("body_xyz") #print(body_xyz, qx,qy,qz,qw) z = body_xyz[2] self.distance = body_xyz[0] if self.initial_z == None: self.initial_z = z (vx, vy, vz), _ = p.getBaseVelocity(human) more = np.array( [z - self.initial_z, 0.1 * vx, 0.1 * vy, 0.1 * vz, qx, qy, qz, qw]) rcont = p.getContactPoints(human, -1, self.right_foot, -1) #print("rcont") #print(rcont) lcont = p.getContactPoints(human, -1, self.left_foot, -1) #print("lcont") #print(lcont) feet_contact = np.array([len(rcont) > 0, len(lcont) > 0]) return np.clip(np.concatenate([more] + [j] + [feet_contact]), -5, +5)
def get_state(self): # Returns a pinocchio like representation of the q, dq matrixes q = zero(self.nq) dq = zero(self.nv) pos, orn = p.getBasePositionAndOrientation(self.robot_id) q[:3, 0] = np.array(pos).reshape(3, 1) q[3:7, 0] = np.array(orn).reshape(4, 1) vel, orn = p.getBaseVelocity(self.robot_id) dq[:3, 0] = np.array(vel).reshape(3, 1) dq[3:6, 0] = np.array(orn).reshape(3, 1) # Pinocchio assumes the base velocity to be in the body frame -> rotate. rot = np.matrix(p.getMatrixFromQuaternion(q[3:7])).reshape((3, 3)) dq[0:3] = rot.T.dot(dq[0:3]) dq[3:6] = rot.T.dot(dq[3:6]) # Query the joint readings. joint_states = p.getJointStates(self.robot_id, self.bullet_joint_ids) for i in range(self.nj): q[5 + self.pinocchio_joint_ids[i], 0] = joint_states[i][0] dq[4 + self.pinocchio_joint_ids[i], 0] = joint_states[i][1] return q, dq
def _getObservation(self): allJoints = [j.value for j in Joints] jointStates = p.getJointStates(self.anymal, allJoints) position = np.array([js[0] for js in jointStates]) velocity = np.array([js[1] for js in jointStates]) global lastVelocity if self.t == 0.0: lastVelocity = velocity acceleration = (velocity - lastVelocity) * SIMULATIONFREQUENCY lastVelocity = velocity basePosition, baseOrientation = p.getBasePositionAndOrientation(self.anymal) baseOrientation_Matrix = np.array(p.getMatrixFromQuaternion(baseOrientation)).reshape(3, 3) baseOrientationVector = np.matmul(baseOrientation_Matrix, [0, 0, -1]) baseVelocity, baseAngularVelocity = p.getBaseVelocity(self.anymal) observationAsArray = np.concatenate([ position, velocity, basePosition, baseOrientation, baseOrientationVector, baseVelocity, baseAngularVelocity, acceleration ]) observationAsDict = { "position": position, "velocity": velocity, "basePosition": basePosition, "baseOrientation": baseOrientation, "baseOrientationVector": baseOrientationVector, "baseVelocity": baseVelocity, "baseAngularVelocity": baseAngularVelocity, "acceleration": acceleration } return observationAsArray, observationAsDict
def step(self, action): self.take_step(action, robot_arm='right', gains=self.config('robot_gains'), forces=self.config('robot_forces'), human_gains=0.0005) robot_force_on_human, cup_force_on_human = self.get_total_force() total_force_on_human = robot_force_on_human + cup_force_on_human reward_water, water_mouth_velocities, water_hit_human_reward = self.get_water_rewards() end_effector_velocity = np.linalg.norm(p.getBaseVelocity(self.cup, physicsClientId=self.id)[0]) obs = self._get_obs([cup_force_on_human], [robot_force_on_human, cup_force_on_human]) # Get human preferences preferences_score = self.human_preferences(end_effector_velocity=end_effector_velocity, total_force_on_human=robot_force_on_human, tool_force_at_target=cup_force_on_human, food_hit_human_reward=water_hit_human_reward, food_mouth_velocities=water_mouth_velocities) cup_pos, cup_orient = p.getBasePositionAndOrientation(self.cup, physicsClientId=self.id) cup_pos, cup_orient = p.multiplyTransforms(cup_pos, cup_orient, [0, 0.06, 0], p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id) cup_top_center_pos, _ = p.multiplyTransforms(cup_pos, cup_orient, self.cup_top_center_offset, [0, 0, 0, 1], physicsClientId=self.id) reward_distance = -np.linalg.norm(self.target_pos - np.array(cup_top_center_pos)) # Penalize distances between top of cup and mouth reward_action = -np.sum(np.square(action)) # Penalize actions # Encourage robot to have a tilted end effector / cup cup_euler = p.getEulerFromQuaternion(cup_orient, physicsClientId=self.id) reward_tilt = -abs(cup_euler[0] + np.pi/2) if self.robot_type == 'jaco' else -abs(cup_euler[0] - np.pi/2) reward = self.config('distance_weight')*reward_distance + self.config('action_weight')*reward_action + self.config('cup_tilt_weight')*reward_tilt + self.config('drinking_reward_weight')*reward_water + preferences_score if self.gui and reward_water != 0: print('Task success:', self.task_success, 'Water reward:', reward_water) info = {'total_force_on_human': total_force_on_human, 'task_success': int(self.task_success >= self.total_water_count*self.config('task_success_threshold')), 'action_robot_len': self.action_robot_len, 'action_human_len': self.action_human_len, 'obs_robot_len': self.obs_robot_len, 'obs_human_len': self.obs_human_len} done = False return obs, reward, done, info
def dumpStateToFile(file): for i in range (p.getNumBodies()): pos,orn = p.getBasePositionAndOrientation(i) linVel,angVel = p.getBaseVelocity(i) txtPos = "pos="+str(pos)+"\n" txtOrn = "orn="+str(orn)+"\n" txtLinVel = "linVel"+str(linVel)+"\n" txtAngVel = "angVel"+str(angVel)+"\n" file.write(txtPos) file.write(txtOrn) file.write(txtLinVel) file.write(txtAngVel)
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 speed(self): if self.bodyPartIndex == -1: (vx, vy, vz), _ = p.getBaseVelocity(self.bodies[self.bodyIndex]) else: (x,y,z), (a,b,c,d), _,_,_,_, (vx, vy, vz), (vr,vp,vy) = p.getLinkState(self.bodies[self.bodyIndex], self.bodyPartIndex, computeLinkVelocity=1) return np.array([vx, vy, vz])
updateCam=1 time.sleep(timestep) pts = p.getContactPoints() #print("numPoints=",len(pts)) #for point in pts: # print("Point:bodyA=", point[1],"bodyB=",point[2],"linkA=",point[3],"linkB=",point[4],"dist=",point[8],"force=",point[9]) states.append(p.saveState()) #observations.append(obs) stateIndex = len(states) #print("stateIndex =",stateIndex ) frame += 1 if (updateCam): distance=5 yaw = 0 humanPos, humanOrn = p.getBasePositionAndOrientation(bike) humanBaseVel = p.getBaseVelocity(bike) #print("frame",frame, "humanPos=",humanPos, "humanVel=",humanBaseVel) if (gui): camInfo = p.getDebugVisualizerCamera() curTargetPos = camInfo[11] distance=camInfo[10] yaw = camInfo[8] pitch=camInfo[9] targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]] p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos);