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 closeGripper(gripperID, targetID): contact = p.getContactPoints(gripperID, targetID) fourPos = p.getJointState(gripperID, 4)[0] sixPos = p.getJointState(gripperID, 6)[0] targetVelocity = 0.02 while not contact: p.setJointMotorControl2(gripperID, 4, p.VELOCITY_CONTROL, targetVelocity=targetVelocity) p.setJointMotorControl2(gripperID, 6, p.VELOCITY_CONTROL, targetVelocity=targetVelocity) stepSim(50) contact = p.getContactPoints(gripperID, targetID) p.setJointMotorControl2(gripperID, 4, p.VELOCITY_CONTROL, targetVelocity=0) p.setJointMotorControl2(gripperID, 6, p.VELOCITY_CONTROL, targetVelocity=0) fourPos = p.getJointState(gripperID, 4)[0] sixPos = p.getJointState(gripperID, 6)[0] pos = max(fourPos, sixPos) p.setJointMotorControl2(gripperID, 4, p.POSITION_CONTROL, targetPosition=pos + 0.0125, force=100) p.setJointMotorControl2(gripperID, 6, p.POSITION_CONTROL, targetPosition=pos + 0.0125, force=100)
def _getReward(self): target = self.target objectPosition = self.objectPosition tip = self.robot.joints[self.fingerTipName].getWorldPosition() tip_target = [0, 0, 0] norm = np.sqrt((np.power(target[0] - objectPosition[0], 2)) + (np.power(target[1] - objectPosition[1], 2))) tip_target[0] = objectPosition[0] - ( (target[0] - objectPosition[0]) / norm) * (self.r) tip_target[1] = objectPosition[1] - ( (target[1] - objectPosition[1]) / norm) * (self.r) tip_target[2] = objectPosition[2] tip_target2 = [0, 0, 0] tip_target2[0] = objectPosition[0] - ( (target[0] - objectPosition[0]) / norm) * (self.r) tip_target2[1] = objectPosition[1] - ( (target[1] - objectPosition[1]) / norm) * (self.r) tip_target2[2] = objectPosition[2] tip_distance = math.sqrt( sum((x - y)**2 for x, y in zip(tip, tip_target))) object_distance = math.sqrt( sum((x - y)**2 for x, y in itertools.islice( zip(self.objectPosition, target), 2))) table_contact = pb.getContactPoints( self.robot.joints[self.fingerTipName].bodyIndex, self.table.id) cp = pb.getContactPoints( self.robot.joints[self.fingerTipName].bodyIndex, self.box.objId) contact = False reward = 0 sparse_reward = 0 if cp: contact = True if self.reward_type == 'sparse': reward = int(object_distance <= 0.05) elif self.reward_type == 'shaped': reward = 0 cp = pb.getContactPoints( self.robot.joints[self.fingerTipName].bodyIndex, self.box.objId) reward = -tip_distance / 3 + -object_distance contact = False if cp: reward = -object_distance contact = True if object_distance < 0.05: reward = 1 sparse_reward = 1 if self.reward_type == "shaped": return reward elif self.reward_type == "sparse": return sparse_reward
def get_force(self): """ Returns the force readings as well as the set of active contacts """ active_contacts_frame_ids = [] contact_forces = [] for i in self.bullet_endeff_ids: cpA = p.getContactPoints(bodyA=self.robot_id, linkIndexA=i) cpB = p.getContactPoints(bodyB=self.robot_id, linkIndexB=i) cp = cpA + cpB if len(cp) > 0: for ci in cp: contact_normal = ci[7] normal_force = ci[9] lateral_friction_direction_1 = ci[11] lateral_friction_force_1 = ci[10] lateral_friction_direction_2 = ci[13] lateral_friction_force_2 = ci[12] j = np.where(np.array(self.bullet_endeff_ids) == i)[0][0] active_contacts_frame_ids.append( self.pinocchio_endeff_ids[j]) force = np.zeros(6) force[:3] = normal_force * np.array(contact_normal) + \ lateral_friction_force_1 * np.array(lateral_friction_direction_1) + \ lateral_friction_force_2 * np.array(lateral_friction_direction_2) contact_forces.append(force) return active_contacts_frame_ids, contact_forces
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.008 # closestPoints = p.getClosestPoints(self.blockUid, self._kuka.kukaUid, maxDist) closestPoints = p.getClosestPoints(self.blockUid, self._kuka.kukaUid, maxDist, -1, 13) contactPoints = p.getContactPoints(self.blockUid, self._kuka.kukaUid) contactPoints_tray = p.getContactPoints(self._kuka.trayUid, self._kuka.kukaUid) if ((len(contactPoints)) and contactPoints[0][8] < 0) or len(closestPoints): self.terminated = 1 print('********** Terminating Normally...') self._observation = self.getExtendedObservation() return True #elif (len(contactPoints_tray) and contactPoints_tray[0][8]<0): #self._observation = self.getExtendedObservation() # print('********* Terminating by contacting the tray!!!') # return True return False
def get_handcraft_reward(self): reward = 0 # check whether the seqence number is out of limit if self.seq_num >= self.max_seq_num: done = True else: done = False # check whether the object are out of order for axis_dim in range(3): if self.start_pos[axis_dim] < self.axis_limit[axis_dim][0] or \ self.start_pos[axis_dim] > self.axis_limit[axis_dim][1]: done = True reward = self.opt.out_reward # check whether the object is still in the gripper left_closet_info = p.getContactPoints(self.robotId, self.obj_id, 13, -1) right_closet_info = p.getContactPoints(self.robotId, self.obj_id, 17, -1) if self.opt.obj_away_loss: if len(left_closet_info) == 0 and len(right_closet_info) == 0: done = True # let the model learn the reward automatically, so delete the following line reward = self.opt.away_reward self.info += 'reward: {}\n\n'.format(reward) self.log_info.write(self.info) print(self.info) return self.observation, reward, done, self.info
def get_total_force(self): tool_left_force = 0 tool_right_force = 0 total_force_on_human = 0 tool_left_force_on_human = 0 tool_right_force_on_human = 0 for c in p.getContactPoints(bodyA=self.robot, physicsClientId=self.id): linkA = c[3] if linkA == (55 if self.robot_type == 'pr2' else 24 if self.robot_type == 'sawyer' else 31 if self.robot_type == 'baxter' else 9 if self.robot_type == 'jaco' else 7): tool_right_force += c[9] elif linkA == (78 if self.robot_type == 'pr2' else 24 if self.robot_type == 'sawyer' else 54 if self.robot_type == 'baxter' else 9 if self.robot_type == 'jaco' else 7): tool_left_force += c[9] for c in p.getContactPoints(bodyA=self.robot, bodyB=self.human, physicsClientId=self.id): total_force_on_human += c[9] linkA = c[3] linkB = c[4] if linkA == (55 if self.robot_type == 'pr2' else 24 if self.robot_type == 'sawyer' else 31 if self.robot_type == 'baxter' else 9 if self.robot_type == 'jaco' else 7): tool_right_force_on_human += c[9] elif linkA == (78 if self.robot_type == 'pr2' else 24 if self.robot_type == 'sawyer' else 54 if self.robot_type == 'baxter' else 9 if self.robot_type == 'jaco' else 7): tool_left_force_on_human += c[9] return tool_left_force, tool_right_force, total_force_on_human, tool_left_force_on_human, tool_right_force_on_human
def get_food_rewards(self): # Check all food particles to see if they have left the spoon or entered the person's mouth # Give the robot a reward or penalty depending on food particle status food_reward = 0 food_hit_human_reward = 0 food_mouth_velocities = [] foods_to_remove = [] for f in self.foods: food_pos, food_orient = p.getBasePositionAndOrientation(f, physicsClientId=self.id) distance_to_mouth = np.linalg.norm(self.target_pos - food_pos) if distance_to_mouth < 0.02: # Delete particle and give robot a reward food_reward += 20 self.task_success += 1 food_velocity = np.linalg.norm(p.getBaseVelocity(f, physicsClientId=self.id)[0]) food_mouth_velocities.append(food_velocity) foods_to_remove.append(f) p.resetBasePositionAndOrientation(f, self.np_random.uniform(1000, 2000, size=3), [0, 0, 0, 1], physicsClientId=self.id) continue elif food_pos[-1] < 0.5 or len(p.getContactPoints(bodyA=f, bodyB=self.table, physicsClientId=self.id)) > 0 or len(p.getContactPoints(bodyA=f, bodyB=self.bowl, physicsClientId=self.id)) > 0: # Delete particle and give robot a penalty for spilling food food_reward -= 5 foods_to_remove.append(f) continue if len(p.getContactPoints(bodyA=f, bodyB=self.human, physicsClientId=self.id)) > 0 and f not in self.foods_hit_person: # Record that this food particle just hit the person, so that we can penalize the robot self.foods_hit_person.append(f) food_hit_human_reward -= 1 self.foods = [f for f in self.foods if f not in foods_to_remove] return food_reward, food_mouth_velocities, food_hit_human_reward
def get_foot_pressure_sensors(self, floor): """ Checks if 4 corners of the each feet are in contact with ground Indicies for looking from above on the feet plates: Left Right 4-------5 0-------1 | ^ | | ^ | ^ | | | | | | | : forward direction | | | | 6-------7 2-------3 :param floor: PyBullet body id of the plane the robot is walking on. :return: boolean array of 8 contact points on both feet, True: that point is touching the ground False: otherwise """ locations = [False] * 8 right_pts = pb.getContactPoints(bodyA=self.body, bodyB=floor, linkIndexA=Links.RIGHT_LEG_6) left_pts = pb.getContactPoints(bodyA=self.body, bodyB=floor, linkIndexA=Links.LEFT_LEG_6) right_center = np.array(pb.getLinkState(self.body, linkIndex=Links.RIGHT_LEG_6)[4]) left_center = np.array(pb.getLinkState(self.body, linkIndex=Links.LEFT_LEG_6)[4]) right_tr = tr.get_rotation_matrix_from_transformation( tr(quaternion=pb.getLinkState(self.body, linkIndex=Links.RIGHT_LEG_6)[5])) left_tr = tr.get_rotation_matrix_from_transformation( tr(quaternion=pb.getLinkState(self.body, linkIndex=Links.LEFT_LEG_6)[5])) for point in right_pts: index = np.signbit(np.matmul(right_tr, point[5] - right_center))[0:2] locations[index[1] + index[0] * 2] = True for point in left_pts: index = np.signbit(np.matmul(left_tr, point[5] - left_center))[0:2] locations[index[1] + (index[0] * 2) + 4] = True return locations
def get_video_reward(self): if ((self.seq_num-1)%self.opt.give_reward_num==self.opt.give_reward_num-1) \ and self.seq_num>=self.opt.cut_frame_num: if self.opt.use_cycle: self.cycle.image_transfer(self.epoch_num) self.eval.update(img_path=self.log_path, start_id=self.seq_num - 1 - self.opt.cut_frame_num) # self.eval.get_caption() rank, probability = self.eval.eval(self.img_buffer) reward = probability self.info += 'rank: {}\n'.format(rank) else: reward = 0 if self.seq_num >= self.max_seq_num: done = True else: done = False # check whether the object is still in the gripper left_closet_info = p.getContactPoints(self.robotId, self.obj_id, 13, -1) right_closet_info = p.getContactPoints(self.robotId, self.obj_id, 17, -1) if self.opt.obj_away_loss: if len(left_closet_info) == 0 and len(right_closet_info) == 0: done = True self.info += 'reward: {}\n\n'.format(reward) # self.log_info.write (self.info) # print (self.info) return self.observation, reward, done, self.info
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 findContact(self): self.contactBetweenBallAndGround = p.getContactPoints( bodyA=self.ballId, bodyB=self.planeId) self.contactBetweenBallAndRobot = p.getContactPoints( bodyA=self.ballId, bodyB=self.robotId) if self.contactBetweenBallAndRobot: self.catchFlag = True
def get_food_rewards(self): food_reward = 0 foods_to_remove = [] sum_dis = 0 for f in self.foods: food_pos, food_orient = p.getBasePositionAndOrientation( f, physicsClientId=self.id) distance_to_mouth = np.linalg.norm(self.target_pos - food_pos) sum_dis += distance_to_mouth if distance_to_mouth < 0.03: # Delete particle and give robot a reward food_reward += 20 self.task_success += 1 foods_to_remove.append(f) p.resetBasePositionAndOrientation( f, self.np_random.uniform(1000, 2000, size=3), [0, 0, 0, 1], physicsClientId=self.id) #重置foodposition,远去 continue elif food_pos[-1] < 0.5 or len( p.getContactPoints( bodyA=f, bodyB=self.table, physicsClientId=self.id)) > 0 or len( p.getContactPoints(bodyA=f, bodyB=self.bowl, physicsClientId=self.id)) > 0: # Delete particle and give robot a penalty for spilling food food_reward -= 5 self.fall_time += 1 foods_to_remove.append(f) mean_dis = sum_dis / len(self.foods) self.foods = [f for f in self.foods if f not in foods_to_remove] return food_reward, mean_dis
def _reward(self): #rewards is height of target object blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid) closestPoints = p.getClosestPoints(self.blockUid, self._kuka.kukaUid, 1000, -1, 13) """ EndEffector distance is: 0.39769955968108894 Gripper distance is: 0.36823755806508035 """ reward = -1000 # reward = -500 numPt = len(closestPoints) contactPoints = p.getContactPoints(self.blockUid, self._kuka.kukaUid) contactPoints_tray = p.getContactPoints(self._kuka.trayUid, self._kuka.kukaUid) #print(numPt) if (numPt > 0): reward = -closestPoints[0][8] * 10 if closestPoints[0][8] < 0.008 or (len(contactPoints) and contactPoints[0][8] < 0): reward += 1000 print( '******Less than 0.008! OR contacted the object, adding 1000********' ) #elif (len(contactPoints_tray) and contactPoints_tray[0][8]<0): # print('Because of contacting the tray, the reward will be reduced by 1000!') #reward=reward-1000 return reward
def _feet(self): """ Checks if 4 corners of the each feet are in contact with ground Indicies for looking from above on the feet plates: Left Right 4-------5 0-------1 | ^ | | ^ | ^ | | | | | | | : forward direction | | | | 6-------7 2-------3 :return: int array of 8 contact points on both feet, 1: that point is touching the ground -1: otherwise """ locations = [-1.] * self._FEET_DIM right_pts = pb.getContactPoints(bodyA=self.soccerbotUid, bodyB=self.planeUid, linkIndexA=Links.RIGHT_LEG_6) left_pts = pb.getContactPoints(bodyA=self.soccerbotUid, bodyB=self.planeUid, linkIndexA=Links.LEFT_LEG_6) right_center = np.array(pb.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6)[4]) left_center = np.array(pb.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.LEFT_LEG_6)[4]) right_tr = np.array(pb.getMatrixFromQuaternion( pb.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6)[5]) , dtype=self.DTYPE).reshape((3, 3)) left_tr = np.array(pb.getMatrixFromQuaternion( pb.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.LEFT_LEG_6)[5]) , dtype=self.DTYPE).reshape((3, 3)) for point in right_pts: index = np.signbit(np.matmul(right_tr, point[5] - right_center))[0:2] locations[index[1] + index[0] * 2] = 1. for point in left_pts: index = np.signbit(np.matmul(left_tr, point[5] - left_center))[0:2] locations[index[1] + (index[0] * 2) + 4] = 1. for i in range(len(locations)): # 5% chance of incorrect reading locations[i] *= np.sign(self.np_random.uniform(low=- self._FEET_FALSE_CHANCE, high=1 - (self._FEET_FALSE_CHANCE)), dtype=self.DTYPE) return np.array(locations)
def get_total_force(self): robot_force_on_human = 0 cup_force_on_human = 0 for c in p.getContactPoints(bodyA=self.robot, bodyB=self.human, physicsClientId=self.id): robot_force_on_human += c[9] for c in p.getContactPoints(bodyA=self.cup, bodyB=self.human, physicsClientId=self.id): cup_force_on_human += c[9] return robot_force_on_human, cup_force_on_human
def get_obs(self): ''' Returns a suite of observations of the given state of the robot. Not all of these are required. :return: list of lists ''' # Torso torso_pos, torso_quat = p.getBasePositionAndOrientation( self.robot, physicsClientId=self.client_ID) # xyz and quat: x,y,z,w torso_vel, torso_angular_vel = p.getBaseVelocity( self.robot, physicsClientId=self.client_ID) #distance of each foot to target position """ l_vec_target = p.getLinkState(self.robot, self.left_foot)[0] - self.l_foot_target ctct_l = np.linalg.norm(l_vec_target) r_vec_target = p.getLinkState(self.robot, self.right_foot)[0] - self.r_foot_target ctct_r = np.linalg.norm(r_vec_target) contacts = [ctct_l, ctct_r] """ # Joints obs = p.getJointStates(self.robot, self.joints_index, physicsClientId=self.client_ID ) # pos, vel, reaction(6), prev_torque joint_angles = [] joint_velocities = [] joint_torques = [] for o in obs: joint_angles.append(o[0]) joint_velocities.append(o[1]) joint_torques.append(o[3]) # add positions of links to observation instead of contacts> # [l[4] for l in p.getLinkStates(self.robot, range(0, 13+1), physicsClientId=self.client_ID)] # link positions and contacts """ obs = p.getLinkStates(self.robot, range(p.getNumJoints(self.robot)), physicsClientId=self.client_ID) link_pos_orient = [] for o in obs: link_pos_orient.append(o[1]) """ a = p.getNumJoints(self.robot) # 0/1 contact of each link to car and floor contacts = [0] * (a * 2) obs = p.getContactPoints(self.robot, self.car, physicsClientId=self.client_ID) for o in obs: contacts[o[ 3]] = 1 # for each contact point, store 1 at index of participating robot's link obs = p.getContactPoints(self.robot, self.plane, physicsClientId=self.client_ID) for o in obs: contacts[a + o[ 3]] = 1 # for each contact point, store 1 at index of participating robot's link return torso_pos, torso_quat, torso_vel, torso_angular_vel, joint_angles, joint_velocities, joint_torques, contacts
def get_handcraft_reward (self): distance = sum ([(x - y) ** 2 for x, y in zip (self.start_pos, self.target_pos)]) ** 0.5 box = p.getAABB (self.box_id, -1) box_center = [(x + y) * 0.5 for x, y in zip (box[0], box[1])] obj = p.getAABB (self.obj_id, -1) obj_center = [(x + y) * 0.5 for x, y in zip (obj[0], obj[1])] aabb_dist = sum ([(x - y) ** 2 for x, y in zip (box_center, obj_center)]) ** 0.5 if self.opt.video_reward: if ((self.seq_num-1)%self.opt.give_reward_num==self.opt.give_reward_num-1) \ and self.seq_num>=self.opt.cut_frame_num: self.eval.get_caption() rank,probability = self.eval.eval() reward = probability self.info += 'rank: {}\n'.format(rank) self.eval.update(img_path=self.log_path,start_id=self.seq_num-1-self.opt.cut_frame_num) else: reward = 0 else: if self.opt.reward_diff: reward = (self.last_aabb_dist - aabb_dist) * 100 else: reward = (0.5-aabb_dist)*100 # reward = (self.last_aabb_dist-aabb_dist)*100 self.last_aabb_dist = aabb_dist # calculate whether it is done self.info += 'now distance:{}\n'.format (distance) self.info += 'AABB distance:{}\n'.format (aabb_dist) # check whether the seqence number is out of limit if self.seq_num >= self.max_seq_num: done = True else: done = False # check whether the object are out of order for axis_dim in range (3): if self.start_pos[axis_dim] < self.axis_limit[axis_dim][0] or \ self.start_pos[axis_dim] > self.axis_limit[axis_dim][1]: done = True reward = self.opt.out_reward # check whether the object is still in the gripper left_closet_info = p.getContactPoints (self.robotId, self.obj_id, 13, -1) right_closet_info = p.getContactPoints (self.robotId, self.obj_id, 17, -1) if self.opt.obj_away_loss: if len (left_closet_info)==0 and len (right_closet_info)==0: done = True # let the model learn the reward automatically, so delete the following line reward = self.opt.away_reward self.info += 'reward: {}\n\n'.format (reward) self.log_info.write (self.info) print (self.info) return self.observation, reward, done, self.info
def display_contact_forces(self): # Info about contact points with the ground contactPoints_FL = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=3) # Front left foot contactPoints_FR = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=7) # Front right foot contactPoints_HL = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=11) # Hind left foot contactPoints_HR = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=15) # Hind right foot # print(len(contactPoint_FL), len(contactPoint_FR), len(contactPoint_HL), len(contactPoint_HR)) # Sort contacts points to get only one contact per foot contactPoints = [] contactPoints.append(self.getContactPoint(contactPoints_FL)) contactPoints.append(self.getContactPoint(contactPoints_FR)) contactPoints.append(self.getContactPoint(contactPoints_HL)) contactPoints.append(self.getContactPoint(contactPoints_HR)) # Display debug lines for contact forces visualization i_line = 0 # print(len(self.lines)) f_x = 0 f_y = 0 f_z = 0 f_tmp = [0.0] * 3 for contact in contactPoints: if not isinstance(contact, int): # type(contact) != type(0): start = [contact[6][0], contact[6][1], contact[6][2]+0.04] end = [contact[6][0], contact[6][1], contact[6][2]+0.04] K = 0.02 for i_direction in range(0, 3): f_tmp[i_direction] = (contact[9] * contact[7][i_direction] + contact[10] * contact[11][i_direction] + contact[12] * contact[13][i_direction]) end[i_direction] += K * f_tmp[i_direction] """if contact[3] < 10: print("Link ", contact[3], "| Contact force: (", f_tmp[0], ", ", f_tmp[1], ", ", f_tmp[2], ")") else: print("Link ", contact[3], "| Contact force: (", f_tmp[0], ", ", f_tmp[1], ", ", f_tmp[2], ")")""" f_x += f_tmp[0] f_y += f_tmp[1] f_z += f_tmp[2] if (i_line+1) > len(self.lines): # If not enough existing lines in line storage a new item is created lineID = pyb.addUserDebugLine(start, end, lineColorRGB=[1.0, 0.0, 0.0], lineWidth=8) self.lines.append(lineID) else: # If there is already an existing line item we modify it (to avoid flickering) self.lines[i_line] = pyb.addUserDebugLine(start, end, lineColorRGB=[ 1.0, 0.0, 0.0], lineWidth=8, replaceItemUniqueId=self.lines[i_line]) i_line += 1 # Should be around 21,5 (2.2 kg * 9.81 m^2/s) # print("Total ground reaction force: (", f_x, ", ", f_y, ", ", f_z, ")") for i_zero in range(i_line, len(self.lines)): self.lines[i_zero] = pyb.addUserDebugLine([0.0, 0.0, 0.0], [0.0, 0.0, 0.0], lineColorRGB=[ 1.0, 0.0, 0.0], lineWidth=8, replaceItemUniqueId=self.lines[i_zero])
def get_collision(self, bodyB): outA = p.getContactPoints(bodyA=self.robot.id, bodyB=bodyB, linkIndexA=14, linkIndexB=0) outB = p.getContactPoints(bodyA=self.robot.id, bodyB=bodyB, linkIndexA=13, linkIndexB=0) return len(outA) + len(outB)
def step(self, action): sigmoid = lambda x: 1 / (1 + np.exp(-np.clip(x, -709, 709))) action[0:2] = np.tanh(action[0:2]) action[2] = sigmoid(action[2]) action = [action[mm] * t for mm, t in enumerate(self.max_thrust)] self.apply_thrust(action[2]) self.apply_control_thrust(action[0], action[1]) p.stepSimulation() obs = self.get_obs() reward, info = 0.0, None done = False # check for collision nose cone on ground plane nose_contact_points= p.getContactPoints(self.bot_id, self.plane_id, 1) landing_gear_contacts = [] for link_idx in range(3,7): landing_gear_contacts.append(p.getContactPoints(self.bot_id, self.plane_id, link_idx)) landed = True in [len(elem) > 0 for elem in landing_gear_contacts] if len(nose_contact_points) > 0: #print("nose ground collsion") done = True reward -= self.crash_bonus # nose cone is on the ground elif landed and np.abs(np.mean(obs[-7:-4])) < 1e-4: #print("bell is down") done = True reward += self.land_bonus self.step_count += 1 if self.step_count > self.max_steps: done = True # lost in space #print("lost in space") reward -= self.timeout_bonus if done and len(nose_contact_points) == 0: reward += self.fuel elif not done: # survival bonus reward += self.survival_bonus #if done: print(self.step_count) return obs, reward, done, info
def step(self, action, custom_reward=None): p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING) p.setJointMotorControlArray( self.yumiUid, [self.joint2Index[joint] for joint in self.joints], p.POSITION_CONTROL, action) p.stepSimulation() # get joint states jointStates = {} for joint in self.joints: jointStates[joint] = p.getJointState( self.yumiUid, self.joint2Index[joint]) + p.getLinkState( self.yumiUid, self.joint2Index[joint]) # recover color for joint, index in self.joint2Index.items(): if joint in self.jointColor and joint != 'world_joint': p.changeVisualShape(self.yumiUid, index, rgbaColor=self.jointColor[joint]) # check collision collision = False for joint in self.joints: if len( p.getContactPoints( bodyA=self.yumiUid, linkIndexA=self.joint2Index[joint])) > 0: collision = True for contact in p.getContactPoints( bodyA=self.yumiUid, linkIndexA=self.joint2Index[joint]): print( "Collision Occurred in Joint {} & Joint {}!!!".format( contact[3], contact[4])) p.changeVisualShape(self.yumiUid, contact[3], rgbaColor=[1, 0, 0, 1]) p.changeVisualShape(self.yumiUid, contact[4], rgbaColor=[1, 0, 0, 1]) self.step_counter += 1 if custom_reward is None: # default reward reward = 0 done = False else: # custom reward reward, done = custom_reward(jointStates=jointStates, collision=collision, step_counter=self.step_counter) info = {'collision': collision} observation = [jointStates[joint][0] for joint in self.joints] return observation, reward, done, info
def check_grasped_id(self): left_index = self.joints['left_inner_finger_pad_joint'].id right_index = self.joints['right_inner_finger_pad_joint'].id contact_left = p.getContactPoints(bodyA=self.robotID, linkIndexA=left_index) contact_right = p.getContactPoints(bodyA=self.robotID, linkIndexA=right_index) contact_ids = set(item[2] for item in contact_left + contact_right if item[2] in self.obj_ids) if len(contact_ids) > 1: print('Warning: Multiple items in hand!') if len(contact_ids) == 0: print(contact_left, contact_right) return list(item_id for item_id in contact_ids if item_id in self.obj_ids)
def get_handcraft_reward(self): distance = sum([(x - y)**2 for x, y in zip(self.start_pos, self.target_pos)])**0.5 box = p.getAABB(self.box_id, -1) box_center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])] obj = p.getAABB(self.obj_id, -1) obj_center = [(x + y) * 0.5 for x, y in zip(obj[0], obj[1])] aabb_dist = sum([(x - y)**2 for x, y in zip(box_center, obj_center)])**0.5 if self.opt.reward_diff: reward = (self.last_aabb_dist - aabb_dist) * 100 else: reward = (0.5 - aabb_dist) * 100 # calculate whether it is done self.info += 'now distance:{}\n'.format(distance) self.info += 'AABB distance:{}\n'.format(aabb_dist) if self.seq_num >= self.max_seq_num: done = True else: done = False # check whether the object are out of order for axis_dim in range(3): if self.start_pos[axis_dim] < self.axis_limit[axis_dim][0] or \ self.start_pos[axis_dim] > self.axis_limit[axis_dim][1]: done = True reward = self.opt.out_reward # check whether the object is still in the gripper left_closet_info = p.getContactPoints(self.robotId, self.obj_id, 13, -1) right_closet_info = p.getContactPoints(self.robotId, self.obj_id, 17, -1) if self.opt.obj_away_loss: if len(left_closet_info) == 0 and len(right_closet_info) == 0: done = True # let the model learn the reward automatically, so delete the following line # reward = self.opt.away_reward # if aabb_dist<self.opt.end_distance and abs(max(box[0][2],box[1][2])-min(obj[0][2],obj[1][2]))<0.05: if aabb_dist < self.opt.end_distance: done = True if (not self.opt.video_reward): reward = 100 # reward = -1 self.info += 'reward: {}\n\n'.format(reward) self.log_info.write(self.info) print(self.info) return self.observation, reward, done, self.info
def get_obs(self): # Torso torso_pos, torso_quat = p.getBasePositionAndOrientation( self.robot, physicsClientId=self.client_ID) # xyz and quat: x,y,z,w torso_vel, torso_angular_vel = p.getBaseVelocity( self.robot, physicsClientId=self.client_ID) ctct_leg_1 = int( len( p.getContactPoints(self.robot, self.plane, 2, -1, physicsClientId=self.client_ID)) > 0) * 2 - 1 ctct_leg_2 = int( len( p.getContactPoints(self.robot, self.plane, 5, -1, physicsClientId=self.client_ID)) > 0) * 2 - 1 ctct_leg_3 = int( len( p.getContactPoints(self.robot, self.plane, 8, -1, physicsClientId=self.client_ID)) > 0) * 2 - 1 ctct_leg_4 = int( len( p.getContactPoints(self.robot, self.plane, 11, -1, physicsClientId=self.client_ID)) > 0) * 2 - 1 contacts = [ctct_leg_1, ctct_leg_2, ctct_leg_3, ctct_leg_4] # Joints obs = p.getJointStates(self.robot, range(12), physicsClientId=self.client_ID ) # pos, vel, reaction(6), prev_torque joint_angles = [] joint_velocities = [] joint_torques = [] for o in obs: joint_angles.append(o[0]) joint_velocities.append(o[1]) joint_torques.append(o[3]) return torso_pos, torso_quat, torso_vel, torso_angular_vel, joint_angles, joint_velocities, joint_torques, contacts
def check_grasped(self): left_index = self.joints['left_inner_finger_pad_joint'].id right_index = self.joints['right_inner_finger_pad_joint'].id contact_left = p.getContactPoints(bodyA=self.robot_id, linkIndexA=left_index) contact_right = p.getContactPoints(bodyA=self.robot_id, linkIndexA=right_index) contact_ids = set(item[2] for item in contact_left + contact_right if item[2] in [self.obj_id]) if len(contact_ids) == 1: return True return False
def has_collision(self): """Check if collision happens which involves objects""" for object_id in self.object_ids: if len(p.getContactPoints(object_id)) > 1: return True elif len(p.getContactPoints(object_id)) == 1: contact_point = p.getContactPoints(object_id)[0] contact_normal = contact_point[7] if abs(contact_normal[0]) > .1 or abs(contact_normal[1]) > .1: return True loc, quat = p.getBasePositionAndOrientation(object_id) if -4 < loc[0] < -2.8: return True return False
def _get_foot_contacts(self): LF = 0 if p.getContactPoints( self.quadruped_id, self.plane_id, linkIndexA=self.feet_ids['LF']) == () else 1 RF = 0 if p.getContactPoints( self.quadruped_id, self.plane_id, linkIndexA=self.feet_ids['RF']) == () else 1 LH = 0 if p.getContactPoints( self.quadruped_id, self.plane_id, linkIndexA=self.feet_ids['LH']) == () else 1 RH = 0 if p.getContactPoints( self.quadruped_id, self.plane_id, linkIndexA=self.feet_ids['RH']) == () else 1 return LF, RF, LH, RH
def get_hits(self, robot1=0, robot2=1, links=None): if robot1 is None: hits = p.getContactPoints() elif robot2 is None: hits = p.getContactPoints(robot1) # links=(14,14) for sword+shield elif links is None: hits = p.getContactPoints(robot1, robot2) else: assert len(links) == 2 hits = p.getContactPoints(robot1, robot2, links[0], links[1]) return hits
def sim_run(self, u_l, u_r): base_or_p = np.array(p.getBasePositionAndOrientation(bot)[1]) # pybullet gives quaternions in xyzw format # transforms3d takes quaternions in wxyz format, so you need to shift values b_orient = np.zeros(4) b_orient[0] = base_or_p[3] # w b_orient[1] = base_or_p[0] # x b_orient[2] = base_or_p[1] # y b_orient[3] = base_or_p[2] # z b_orient = transforms3d.quaternions.quat2mat(b_orient) torque = np.zeros(8) torque[0:4] = u_l torque[0] *= -1 # readjust to match motor polarity torque[4:8] = -u_r torque[7] *= -1 # readjust to match motor polarity # print(self.reaction_torques()[0:4]) p.setJointMotorControlArray(bot, jointArray, p.TORQUE_CONTROL, forces=torque) velocities = p.getBaseVelocity(bot) self.v = velocities[ 0] # base linear velocity in global Cartesian coordinates self.omega_xyz = velocities[1] # base angular velocity in Euler XYZ # print(omega_xyz[2], omega_xyz[1], omega_xyz[0]) # base angular velocity in quaternions # self.omega = transforms3d.euler.euler2quat(omega_xyz[0], omega_xyz[1], omega_xyz[2], axes='rxyz') # found to be intrinsic Euler angles (r) # Pull values in from simulator, select relevant ones, reshape to 2D array q = np.reshape([j[0] for j in p.getJointStates(1, range(0, 8))], (-1, 1)) # Detect contact of feet with ground plane c1 = bool(len([c[8] for c in p.getContactPoints(bot, plane, 3)])) c2 = bool(len([c[8] for c in p.getContactPoints(bot, plane, 7)])) # dq = [j[1] for j in p.getJointStates(bot, range(8))] if record_stepped is True: record_stepped(self.out) if useRealTime == 0: p.stepSimulation() return q, b_orient, c1, c2
def __fingerReach(self, gripperId, objectId, finger1LinkId, finger2LinkId, indentationDepth): contactFinger1 = pybullet.getContactPoints(bodyA=gripperId, bodyB=objectId, linkIndexA=finger1LinkId) contactFinger2 = pybullet.getContactPoints(bodyA=gripperId, bodyB=objectId, linkIndexA=finger2LinkId) if len(contactFinger1) < 1 or len(contactFinger2) < 1: return False contactList = contactFinger1 + contactFinger2 for contact in contactList: if contact[8] < indentationDepth: return True return False
maxForce = p.readUserDebugParameter(maxForceId) currentline = line.split(",") #print (currentline) #print("-----") frame = currentline[0] t = currentline[1] #print("frame[",frame,"]") joints=currentline[2:14] #print("joints=",joints) for j in range (12): targetPos = float(joints[j]) p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce) p.stepSimulation() for lower_leg in lower_legs: #print("points for ", quadruped, " link: ", lower_leg) pts = p.getContactPoints(quadruped,-1, lower_leg) #print("num points=",len(pts)) #for pt in pts: # print(pt[9]) time.sleep(1./500.) for j in range (p.getNumJoints(quadruped)): p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0) info = p.getJointInfo(quadruped,j) js = p.getJointState(quadruped,j) #print(info) jointName = info[1] jointType = info[2] if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,(js[0]-jointOffsets[j])/jointDirections[j]))
import pybullet as p p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT p.loadURDF("plane.urdf") p.setGravity(0,0,-10) huskypos = [0,0,0.1] husky = p.loadURDF("husky/husky.urdf",huskypos[0],huskypos[1],huskypos[2]) numJoints = p.getNumJoints(husky) for joint in range (numJoints) : print (p.getJointInfo(husky,joint)) targetVel = 10 #rad/s maxForce = 100 #Newton for joint in range (2,6) : p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce) for step in range (300): p.stepSimulation() targetVel=-10 for joint in range (2,6) : p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce) for step in range (400): p.stepSimulation() p.getContactPoints(husky) p.disconnect()
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()
if (numDifferences>0): print("Error:", numDifferences, " lines are different between files.") else: print("OK, files are identical") 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") dumpStateToFile(file) file.close() ################################# setupWorld() #both restore from file or from in-memory state should work p.restoreState(fileName="state.bullet")
def contact_list(self): return p.getContactPoints(self.bodies[self.bodyIndex], -1, self.bodyPartIndex, -1)
useMaximalCoordinates = True sphereRadius = 0.005 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) mass = 1 visualShapeId = -1 for i in range (5): for j in range (5): for k in range (5): #if (k&2): sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*5*sphereRadius,j*5*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates) #else: # sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates) p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) p.changeDynamics(sphereUid,-1,ccdSweptSphereRadius=0.002) p.setGravity(0,0,-10) pts = p.getContactPoints() print("num points=",len(pts)) print(pts) while (p.isConnected()): time.sleep(1./240.) p.stepSimulation()