def reset(self): objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf")) self.kukaUid = objects[0] #for i in range (p.getNumJoints(self.kukaUid)): # print(p.getJointInfo(self.kukaUid,i)) p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000]) self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ] self.numJoints = p.getNumJoints(self.kukaUid) for jointIndex in range (self.numJoints): p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex]) p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce) self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000) self.endEffectorPos = [0.537,0.0,0.5] self.endEffectorAngle = 0 self.motorNames = [] self.motorIndices = [] for i in range (self.numJoints): jointInfo = p.getJointInfo(self.kukaUid,i) qIndex = jointInfo[3] if qIndex > -1: #print("motorname") #print(jointInfo[1]) self.motorNames.append(str(jointInfo[1])) self.motorIndices.append(i)
def Step(stepIndex): for objectId in range(objectNum): record = log[stepIndex*objectNum+objectId] Id = record[2] pos = [record[3],record[4],record[5]] orn = [record[6],record[7],record[8],record[9]] p.resetBasePositionAndOrientation(Id,pos,orn) numJoints = p.getNumJoints(Id) for i in range (numJoints): jointInfo = p.getJointInfo(Id,i) qIndex = jointInfo[3] if qIndex > -1: p.resetJointState(Id,i,record[qIndex-7+17])
def accurateCalculateInverseKinematics( kukaId, endEffectorId, targetPos, threshold, maxIter): closeEnough = False iter = 0 dist2 = 1e30 while (not closeEnough and iter<maxIter): jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,targetPos) for i in range (numJoints): p.resetJointState(kukaId,i,jointPoses[i]) ls = p.getLinkState(kukaId,kukaEndEffectorIndex) newPos = ls[4] diff = [targetPos[0]-newPos[0],targetPos[1]-newPos[1],targetPos[2]-newPos[2]] dist2 = (diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]) closeEnough = (dist2 < threshold) iter=iter+1 #print ("Num iter: "+str(iter) + "threshold: "+str(dist2)) return jointPoses
def _reset(self): # print("-----------reset simulation---------------") p.resetSimulation() self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0]) self.timeStep = 0.01 p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) p.setGravity(0,0, -10) p.setTimeStep(self.timeStep) p.setRealTimeSimulation(0) initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) p.resetJointState(self.cartpole, 1, initialAngle) p.resetJointState(self.cartpole, 0, initialCartPos) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] return np.array(self.state)
def _reset(self): # print("-----------reset simulation---------------") p.resetSimulation() self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0]) p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0) p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0) p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0) self.timeStep = 0.02 p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0) p.setGravity(0,0, -9.8) p.setTimeStep(self.timeStep) p.setRealTimeSimulation(0) randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) p.resetJointState(self.cartpole, 1, randstate[0], randstate[1]) p.resetJointState(self.cartpole, 0, randstate[2], randstate[3]) #print("randstate=",randstate) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] #print("self.state=", self.state) return np.array(self.state)
def initialize(self): '''''' ur5_urdf_filepath = os.path.join(self.root_dir, 'simulators/urdf/kuka/kuka_with_gripper2.sdf') self.id = pb.loadSDF(ur5_urdf_filepath)[0] pb.resetBasePositionAndOrientation(self.id, [-0.2,0,0], [0,0,0,1]) # self.is_holding = False self.gripper_closed = False self.holding_obj = None self.num_joints = pb.getNumJoints(self.id) [pb.resetJointState(self.id, idx, self.home_positions[idx]) for idx in range(self.num_joints)] self.openGripper() self.arm_joint_names = list() self.arm_joint_indices = list() for i in range (self.num_joints): joint_info = pb.getJointInfo(self.id, i) if i in range(7): self.arm_joint_names.append(str(joint_info[1])) self.arm_joint_indices.append(i)
def initialize(self): '''''' ur5_urdf_filepath = os.path.join( self.root_dir, 'simulators/urdf/ur5/ur5_robotiq_85_gripper_fake.urdf') self.id = pb.loadURDF(ur5_urdf_filepath, [0, 0, 0.1], [0, 0, 0, 1]) # self.is_holding = False self.gripper_closed = False self.holding_obj = None self.num_joints = pb.getNumJoints(self.id) [ pb.resetJointState(self.id, idx, self.home_positions[idx]) for idx in range(self.num_joints) ] self.arm_joint_names = list() self.arm_joint_indices = list() self.gripper_joint_names = list() self.gripper_joint_indices = list() for i in range(self.num_joints): joint_info = pb.getJointInfo(self.id, i) if i in range(1, 7): self.arm_joint_names.append(str(joint_info[1])) self.arm_joint_indices.append(i) elif i in range(10, 12): self.gripper_joint_names.append(str(joint_info[1])) self.gripper_joint_indices.append(i) elif i in range(14, self.num_joints): info = pb.getJointInfo(self.id, i) jointID = info[0] jointName = info[1].decode("utf-8") jointType = jointTypeList[info[2]] jointLowerLimit = info[8] jointUpperLimit = info[9] jointMaxForce = info[10] jointMaxVelocity = info[11] singleInfo = jointInfo(jointID, jointName, jointType, jointLowerLimit, jointUpperLimit, jointMaxForce, jointMaxVelocity) self.robotiq_joints[singleInfo.name] = singleInfo
def resetPose(self): # move z prismatic back to 0 p.setJointMotorControl2(self.gripperUid, self.baseIndex, p.POSITION_CONTROL, targetPosition=-0.05, force=500.0) p.resetJointState(self.gripperUid, self.wristXIndex, targetValue=0) p.resetJointState(self.gripperUid, self.wristYIndex, targetValue=0) p.resetJointState(self.gripperUid, self.wristWIndex, targetValue=0) # reset fingers to home position self.setJointControl(qDes=0.04, maxForce=self.gripperMaxForce)
def reset(self): if not isinstance(self.observation_space, gym.spaces.Dict): raise error.Error( 'GoalEnv requires an observation space of type gym.spaces.Dict' ) for key in ['observation', 'achieved_goal', 'desired_goal']: if key not in self.observation_space.spaces: raise error.Error( 'GoalEnv requires the "{}" key to be part of the observation dictionary.' .format(key)) if self.config["randomize_env"]: self.robot = self.load_robot() self.step_ctr = 0 joint_init_pos_list = self.norm_to_rads([0] * 12) [ p.resetJointState(self.robot, i, joint_init_pos_list[i], 0, physicsClientId=self.client_ID) for i in range(12) ] p.resetBasePositionAndOrientation(self.robot, [0, 0, .15], [0, 0, 0, 1], physicsClientId=self.client_ID) p.setJointMotorControlArray(bodyUniqueId=self.robot, jointIndices=range(12), controlMode=p.POSITION_CONTROL, targetPositions=[0] * 12, forces=[1.4] * 12, physicsClientId=self.client_ID) for i in range(10): p.stepSimulation(physicsClientId=self.client_ID) obs, _, _, _ = self.step(np.zeros(self.act_dim)) return obs
def reset(self): self.episode_reward = 0 self.step_counter = 0 p.resetSimulation() p.configureDebugVisualizer( p.COV_ENABLE_RENDERING, 0) # we will enable rendering after we loaded everything urdfRootPath = pybullet_data.getDataPath() p.setGravity(0, 0, -40) planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane.urdf"), basePosition=[0, 0, -0.65]) rest_poses = [0, -0.215, 0, -2.57, 0, 2.356, 2.356, 0.08, 0.08] self.pandaUid = p.loadURDF(os.path.join(urdfRootPath, "franka_panda/panda.urdf"), useFixedBase=True) for i in range(7): p.resetJointState(self.pandaUid, i, rest_poses[i]) p.resetJointState(self.pandaUid, 9, 0.08) p.resetJointState(self.pandaUid, 10, 0.08) tableUid = p.loadURDF(os.path.join(urdfRootPath, "table/table.urdf"), basePosition=[0.5, 0, -0.65]) trayUid = p.loadURDF(os.path.join(urdfRootPath, "tray/traybox.urdf"), basePosition=[0.65, 0, 0]) # state_object= [random.uniform(0.5,0.8),random.uniform(-0.2,0.2),0.05] state_object = [ 0.75 + random.uniform(-0.05, 0.05), 0 + random.uniform(-0.05, 0.05), 0.08, ] self.object_loc = state_object self.objectUid = p.loadURDF( os.path.join(urdfRootPath, "random_urdfs/000/000.urdf"), basePosition=state_object, ) state_robot = p.getLinkState(self.pandaUid, 11)[0] state_fingers = ( p.getJointState(self.pandaUid, 9)[0], p.getJointState(self.pandaUid, 10)[0], ) self.observation = state_robot + state_fingers + tuple(state_object) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) return np.array(self.observation).astype(np.float32)
def resetRightBackLeg(): #right back leg p.resetJointState(self.quadruped, self.jointNameToId['motor_back_rightL_joint'], self.motorDir[6] * halfpi) p.resetJointState(self.quadruped, self.jointNameToId['knee_back_rightL_link'], self.motorDir[6] * kneeangle) p.resetJointState(self.quadruped, self.jointNameToId['motor_back_rightR_joint'], self.motorDir[7] * halfpi) p.resetJointState(self.quadruped, self.jointNameToId['knee_back_rightR_link'], self.motorDir[7] * kneeangle) p.createConstraint(self.quadruped, self.jointNameToId['knee_back_rightR_link'], self.quadruped, self.jointNameToId['knee_back_rightL_link'], p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.2], [0, 0.01, 0.2]) self.setMotorAngleByName('motor_back_rightL_joint', self.motorDir[6] * halfpi) self.setMotorAngleByName('motor_back_rightR_joint', self.motorDir[7] * halfpi) p.setJointMotorControl2( bodyIndex=self.quadruped, jointIndex=self.jointNameToId['knee_back_rightL_link'], controlMode=p.VELOCITY_CONTROL, targetVelocity=0, force=kneeFrictionForce) p.setJointMotorControl2( bodyIndex=self.quadruped, jointIndex=self.jointNameToId['knee_back_rightR_link'], controlMode=p.VELOCITY_CONTROL, targetVelocity=0, force=kneeFrictionForce)
def setup_human_joints(self, human, joints_positions, controllable_joints, use_static_joints=True, human_reactive_force=None, human_reactive_gain=0.05, non_static_joints=[], add_joint_positions=False): if self.human_impairment != 'tremor': self.human_tremors = np.zeros(len(controllable_joints)) elif len(controllable_joints) == 4: self.human_tremors = self.np_random.uniform(np.deg2rad(-20), np.deg2rad(20), size=len(controllable_joints)) else: self.human_tremors = self.np_random.uniform(np.deg2rad(-10), np.deg2rad(10), size=len(controllable_joints)) # Set starting joint positions human_joint_states = p.getJointStates(human, jointIndices=list(range(p.getNumJoints(human, physicsClientId=self.id))), physicsClientId=self.id) human_joint_positions = np.array([x[0] for x in human_joint_states]) for j in range(p.getNumJoints(human, physicsClientId=self.id)): set_position = None for j_index, j_angle in joints_positions: if j == j_index: if self.vr or self.replay or self.new or self.task == 'bed_bathing': set_position = j_angle if not add_joint_positions else (human_joint_positions[j] + j_angle) p.resetJointState(human, jointIndex=j, targetValue=set_position, targetVelocity=0, physicsClientId=self.id) break else: p.resetJointState(human, jointIndex=j, targetValue=j_angle, targetVelocity=0, physicsClientId=self.id) set_position = j_angle break if use_static_joints and j not in controllable_joints and j not in non_static_joints: # Make all non controllable joints on the person static by setting mass of each link (joint) to 0 p.changeDynamics(human, j, mass=0, physicsClientId=self.id) # Set velocities to 0 p.resetJointState(human, jointIndex=j, targetValue=human_joint_positions[j] if set_position is None else set_position, targetVelocity=0, physicsClientId=self.id) # By default, all joints have motors enabled by default that prevent free motion. Disable these motors in human if not self.vr: if self.replay or self.task == 'bed_bathing': for j in range(p.getNumJoints(human, physicsClientId=self.id)): p.setJointMotorControl2(human, jointIndex=j, controlMode=p.VELOCITY_CONTROL, force=0.1, physicsClientId=self.id) elif not self.new: for j in range(p.getNumJoints(human, physicsClientId=self.id)): p.setJointMotorControl2(human, jointIndex=j, controlMode=p.VELOCITY_CONTROL, force=0, physicsClientId=self.id) self.enforce_joint_limits(human) if human_reactive_force is not None: # NOTE: This runs a Position / Velocity PD controller for each joint motor on the human human_joint_states = p.getJointStates(human, jointIndices=controllable_joints, physicsClientId=self.id) target_human_joint_positions = np.array([x[0] for x in human_joint_states]) forces = [human_reactive_force * self.human_strength] * len(target_human_joint_positions) p.setJointMotorControlArray(human, jointIndices=controllable_joints, controlMode=p.POSITION_CONTROL, targetPositions=target_human_joint_positions, positionGains=np.array([human_reactive_gain]*len(forces)), forces=forces, physicsClientId=self.id)
def reset(self): self.completed = False self.step_counter = 0 p.resetSimulation() p.configureDebugVisualizer( p.COV_ENABLE_RENDERING, 0) # we will enable rendering after we loaded everything urdfRootPath = pybullet_data.getDataPath() p.setGravity(0, 0, -10) planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane.urdf"), basePosition=[0, 0, -0.65]) self.pandaUid = p.loadURDF(os.path.join(urdfRootPath, "franka_panda/panda.urdf"), useFixedBase=True) rest_poses = [0, -0.215, 0, -2.57, 0, 2.356, 2.356, 0.08, 0.08] for i in range(7): p.resetJointState(self.pandaUid, i, rest_poses[i]) p.resetJointState(self.pandaUid, 9, 0.08) p.resetJointState(self.pandaUid, 10, 0.08) tableUid = p.loadURDF(os.path.join(urdfRootPath, "table/table.urdf"), basePosition=[0.5, 0, -0.65]) trayUid = p.loadURDF(os.path.join(urdfRootPath, "tray/traybox.urdf"), basePosition=[0.65, 0, 0]) posObj = [ np.random.uniform(0.5, 0.7), np.random.uniform(-0.15, 0.15), 0.05 ] orientationObj = p.getQuaternionFromEuler( [0, 0, np.random.uniform(-np.pi / 5, np.pi / 5)]) self.objectUid = p.loadURDF(os.path.join(urdfRootPath, "random_urdfs/000/000.urdf"), basePosition=posObj, baseOrientation=orientationObj) # state_object = np.array(state_object) + np.random.uniform(0.05, 0.1, 3) * np.random.choice([-1, 1]) # secondObject = p.loadURDF(os.path.join(urdfRootPath, "random_urdfs/002/002.urdf"), basePosition=state_object) self.observation, _ = self.get_obs() p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) return self.observation
def initialize(self): ur5_urdf_filepath = os.path.join( self.root_dir, 'simulators/urdf/ur5/ur5_simple_gripper.urdf') self.id = pb.loadURDF(ur5_urdf_filepath, [0, 0, 0], [0, 0, 0, 1]) self.gripper_closed = False self.holding_obj = None self.num_joints = pb.getNumJoints(self.id) [ pb.resetJointState(self.id, idx, self.home_positions[idx]) for idx in range(self.num_joints) ] self.arm_joint_names = list() self.arm_joint_indices = list() self.gripper_joint_names = list() self.gripper_joint_indices = list() for i in range(self.num_joints): joint_info = pb.getJointInfo(self.id, i) if i in range(1, 7): self.arm_joint_names.append(str(joint_info[1])) self.arm_joint_indices.append(i) elif i in range(10, 12): self.gripper_joint_names.append(str(joint_info[1])) self.gripper_joint_indices.append(i)
def reset_robot_joints(self, robot): # Reset all robot joints for rj in range(p.getNumJoints(robot, physicsClientId=self.id)): p.resetJointState(robot, jointIndex=rj, targetValue=0, targetVelocity=0, physicsClientId=self.id) # Position end effectors whith dual arm robots for i, j in enumerate(self.robot_left_arm_joint_indices): p.resetJointState(robot, jointIndex=j, targetValue=[1.75, 1.25, 1.5, -0.5, 1, 0, 1][i], targetVelocity=0, physicsClientId=self.id) for i, j in enumerate(self.robot_right_arm_joint_indices): p.resetJointState(robot, jointIndex=j, targetValue=[-1.75, 1.25, -1.5, -0.5, -1, 0, -1][i], targetVelocity=0, physicsClientId=self.id)
def reset(self): self.step_ctr = 0 self.theta_prev = 1 self.target = np.random.rand() * 2 * self.target_var - self.target_var p.resetBasePositionAndOrientation(self.target_vis, [self.target, 0, -1], [0, 0, 0, 1]) self.mass_1, self.mass_2 = self.mass_min + np.random.rand(2) * self.mass_range p.resetJointState(self.cartpole, 0, targetValue=0, targetVelocity=0) p.resetJointState(self.cartpole, 1, targetValue=0, targetVelocity=0) p.resetJointState(self.cartpole, 2, targetValue=0, targetVelocity=0) p.changeDynamics(self.cartpole, 1, mass=self.mass_1) p.changeDynamics(self.cartpole, 2, mass=self.mass_2) p.changeVisualShape(self.cartpole, 1, rgbaColor=[self.mass_1 / (self.mass_min + self.mass_range), 1 - self.mass_1 / (self.mass_min + self.mass_range), 0, 1]) p.changeVisualShape(self.cartpole, 2, rgbaColor=[self.mass_2 / (self.mass_min + self.mass_range), 1 - self.mass_2 / (self.mass_min + self.mass_range), 0, 1]) p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0) p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) p.setJointMotorControl2(self.cartpole, 2, p.VELOCITY_CONTROL, force=0) obs, _, _, _ = self.step(np.zeros(self.act_dim)) return obs
def initializeMotionTarget(self): ''' Convert the motion file into a format that matches the observations collected from the agent in order to compare and compute rewards. ''' 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 ] processedTargetMotion = [] targetFrames = self.targetMotion['Frames'] for frameIndex in range(len(targetFrames)-1): # Set joint positions for joint in self.targetDummy.revoluteJoints: p.resetJointState( self.targetDummyID, jointIndex=joint, targetValue=targetFrames[frameIndex][JointFrameMapIndices[joint]] ) for joint in self.targetDummy.sphericalJoints: p.resetJointStateMultiDof( self.targetDummyID, jointIndex=joint, targetValue=[targetFrames[frameIndex][i] for i in JointFrameMapIndices[3]] ) currentFrame = self.targetDummy.collectObservations() deltaTime = targetFrames[frameIndex][0] if frameIndex < len(targetFrames)-1: # Set joint positions for joint in self.targetDummy.revoluteJoints: p.resetJointState( self.targetDummyID, jointIndex=joint, targetValue=targetFrames[frameIndex+1][JointFrameMapIndices[joint]] ) for joint in self.targetDummy.sphericalJoints: p.resetJointStateMultiDof( self.targetDummyID, jointIndex=joint, targetValue=[targetFrames[frameIndex+1][i] for i in JointFrameMapIndices[3]] ) nextFrame = self.targetDummy.collectObservations() processedTargetMotion.append(self.processVelocities(currentFrame, nextFrame, deltaTime)) return processedTargetMotion
def set_occluder_motion(self, link_id, time): """Set the rotation of the occluder to a specific rotation""" p.resetJointState(self.ground_id, link_id, self.joint_patterns[link_id][time])
def resetPose(self): kneeFrictionForce = 0 halfpi = 1.57079632679 kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length) #left front leg p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle) p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi) self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) #left back leg p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle) p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi) self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) #right front leg p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle) p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi) self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) #right back leg p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle) p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi) self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
def set_state(self, x, vx): p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx)
def train_rnd(use_correlated_noise=False, param_sigma=.0): leg_model_nn = LegModelLSTM(n_inputs=6, n_actions=3) optim = T.optim.Adam(leg_model_nn.parameters(), lr=0.003) print("Training:") ou = OrnsteinUhlenbeckActionNoise(mu=np.zeros(3), sigma=0.5, theta=0.15, dt=1e-1, x0=None) joints = [] acts = [] for iter in range(n_iters): max_velocity = 2.0 - np.random.rand() * param_sigma ou.reset() correlated_noise = [ou() for i in range(episode_len)] ep_joints = [] ep_acts = [] # Sample batchsize episodes [ p.resetJointState(leg, i, np.random.randn(), 0, physicsClientId=client_ID) for i in range(3) ] for st in range(episode_len): # Joint obs = p.getJointStates(leg, range(3), physicsClientId=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]) joint_angles_normed = scale_joints(joint_angles) ep_joints.append(joint_angles_normed) # Action noise = np.random.rand(3) * 2 - 1 if use_correlated_noise: noise = correlated_noise[st] act_normed = noise action_rads = scale_action(act_normed) ep_acts.append(act_normed) # Simulate for i in range(3): p.setJointMotorControl2(bodyUniqueId=leg, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=action_rads[i], force=max_joint_force, positionGain=0.1, velocityGain=0.1, maxVelocity=max_velocity, physicsClientId=client_ID) for i in range(sim_steps_per_iter): p.stepSimulation(physicsClientId=client_ID) if (animate) and True: time.sleep(0.0038) joints.append(ep_joints) acts.append(ep_acts) if iter % batchsize == 0 and iter > 0: # Forward pass joints_T = T.tensor([j[:-1] for j in joints], dtype=T.float32) joints_next_T = T.tensor([j[1:] for j in joints], dtype=T.float32) acts_T = T.tensor([j[:-1] for j in acts], dtype=T.float32) pred, _ = leg_model_nn(T.cat((joints_T, acts_T), dim=2)) joints = [] acts = [] # update optim.zero_grad() loss = F.mse_loss(pred, joints_next_T) loss.backward() optim.step() print("Iter: {}/{}, loss: {}".format(iter, n_iters, loss)) sdir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "legtester.p") T.save(leg_model_nn.state_dict(), sdir) print("Training done")
def resetPose(self): #right front leg self.disableAllMotors() p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2) p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) self.setMotorAngleByName('motor_front_rightR_joint', 1.57) self.setMotorAngleByName('motor_front_rightL_joint',-1.57) #left front leg p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2) p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) self.setMotorAngleByName('motor_front_leftR_joint', 1.57) self.setMotorAngleByName('motor_front_leftL_joint',-1.57) #right back leg p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2) p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) self.setMotorAngleByName('motor_back_rightR_joint', 1.57) self.setMotorAngleByName('motor_back_rightL_joint',-1.57) #left back leg p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2) p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) self.setMotorAngleByName('motor_back_leftR_joint', 1.57) self.setMotorAngleByName('motor_back_leftL_joint',-1.57)
float(value_string_list[1]), float(value_string_list[2]), inverse = True) # pelvis to right leg applyMMMRotationToURDFJoint(human_adult_ID, 3, float(value_string_list[28]), float(value_string_list[29]), float(value_string_list[30])) # pelvis to left leg applyMMMRotationToURDFJoint(human_adult_ID, 4, float(value_string_list[14]), float(value_string_list[15]), float(value_string_list[16])) # right leg to right shin p.resetJointState(human_adult_ID, 5, -float(value_string_list[31])) # left leg to left shin p.resetJointState(human_adult_ID, 6, -float(value_string_list[17])) # right shin to right foot applyMMMRotationToURDFJoint(human_adult_ID, 7, float(value_string_list[23]), float(value_string_list[24]), float(value_string_list[25])) # left shin to left foot applyMMMRotationToURDFJoint(human_adult_ID, 8, float(value_string_list[9]), float(value_string_list[10]), float(value_string_list[11])) # right_shoulder_to_right_arm
import pybullet as p import time import math p.connect(p.SHARED_MEMORY) p.loadURDF("plane.urdf") quadruped = p.loadURDF("quadruped/quadruped.urdf", 0, 0, .3) #p.getNumJoints(1) #right front leg p.resetJointState(quadruped, 0, 1.57) p.resetJointState(quadruped, 2, -2.2) p.resetJointState(quadruped, 3, -1.57) p.resetJointState(quadruped, 5, 2.2) p.createConstraint(quadruped, 2, quadruped, 5, 3, [0, 0, 0], [0, 0.01, 0.2], [0, -0.015, 0.2]) p.setJointMotorControl(quadruped, 0, p.POSITION_CONTROL, 1.57, 1) p.setJointMotorControl(quadruped, 1, p.VELOCITY_CONTROL, 0, 0) p.setJointMotorControl(quadruped, 2, p.VELOCITY_CONTROL, 0, 0) p.setJointMotorControl(quadruped, 3, p.POSITION_CONTROL, -1.57, 1) p.setJointMotorControl(quadruped, 4, p.VELOCITY_CONTROL, 0, 0) p.setJointMotorControl(quadruped, 5, p.VELOCITY_CONTROL, 0, 0) #left front leg p.resetJointState(quadruped, 6, 1.57) p.resetJointState(quadruped, 8, -2.2) p.resetJointState(quadruped, 9, -1.57) p.resetJointState(quadruped, 11, 2.2) p.createConstraint(quadruped, 8, quadruped, 11, 3, [0, 0, 0], [0, -0.01, 0.2], [0, 0.015, 0.2])
p.setJointMotorControl2(quadruped, knee_back_leftR_link, p.POSITION_CONTROL, motordir[3] * (kneeangle + twopi) * float(aa) / steps) p.setJointMotorControl2(quadruped, knee_front_rightL_link, p.POSITION_CONTROL, motordir[4] * (kneeangle) * float(aa) / steps) p.setJointMotorControl2(quadruped, knee_front_rightR_link, p.POSITION_CONTROL, motordir[5] * (kneeangle + twopi) * float(aa) / steps) p.setJointMotorControl2(quadruped, knee_back_rightL_link, p.POSITION_CONTROL, motordir[6] * (kneeangle + twopi) * float(aa) / steps) p.setJointMotorControl2(quadruped, knee_back_rightR_link, p.POSITION_CONTROL, motordir[7] * kneeangle * float(aa) / steps) p.stepSimulation() #time.sleep(fixedTimeStep) else: p.resetJointState(quadruped, motor_front_leftL_joint, motordir[0] * halfpi) p.resetJointState(quadruped, knee_front_leftL_link, motordir[0] * kneeangle) p.resetJointState(quadruped, motor_front_leftR_joint, motordir[1] * halfpi) p.resetJointState(quadruped, knee_front_leftR_link, motordir[1] * kneeangle) p.resetJointState(quadruped, motor_back_leftL_joint, motordir[2] * halfpi) p.resetJointState(quadruped, knee_back_leftL_link, motordir[2] * kneeangle) p.resetJointState(quadruped, motor_back_leftR_joint, motordir[3] * halfpi) p.resetJointState(quadruped, knee_back_leftR_link, motordir[3] * kneeangle) p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi) p.resetJointState(quadruped, knee_front_rightL_link, motordir[4] * kneeangle) p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi) p.resetJointState(quadruped, knee_front_rightR_link, motordir[5] * kneeangle) p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
if (numJoints != 7): exit() #lower limits for null space ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05] #upper limits for null space ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05] #joint ranges for null space jr = [5.8, 4, 5.8, 4, 5.8, 4, 6] #restposes for null space rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0] #joint damping coefficents jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] for i in range(numJoints): p.resetJointState(kukaId, i, rp[i]) p.setGravity(0, 0, 0) t = 0. prevPose = [0, 0, 0] prevPose1 = [0, 0, 0] hasPrevPose = 0 useNullSpace = 1 useOrientation = 1 #If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control. #This can be used to test the IK result accuracy. useSimulation = 0 useRealTimeSimulation = 1 ikSolver = 0 p.setRealTimeSimulation(useRealTimeSimulation)
def reset_position(self, position, velocity): p.resetJointState(self.bodies[self.bodyIndex],self.jointIndex,targetValue=position, targetVelocity=velocity) self.disable_motor()
import math from datetime import datetime from datetime import datetime clid = p.connect(p.SHARED_MEMORY) if (clid<0): p.connect(p.GUI) p.loadURDF("plane.urdf",[0,0,-0.3]) husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659]) for i in range (p.getNumJoints(husky)): print(p.getJointInfo(husky,i)) kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659) ob = kukaId jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ] for jointIndex in range (p.getNumJoints(ob)): p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) #put kuka on top of husky cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1]) baseorn = p.getQuaternionFromEuler([3.1415,0,0.3]) baseorn = [0,0,0,1] #[0, 0, 0.707, 0.707] #p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1]) kukaEndEffectorIndex = 6 numJoints = p.getNumJoints(kukaId) if (numJoints!=7):
def create_human(self, static=True, limit_scale=1.0, specular_color=[0.1, 0.1, 0.1], gender='random', config=None, hipbone_to_mouth_height=None, new=False): if gender not in ['male', 'female']: gender = self.np_random.choice(['male', 'female']) if gender == 'male': hmhs = hipbone_to_mouth_height / 0.6 else: hmhs = hipbone_to_mouth_height / 0.54 def create_body(shape=p.GEOM_CAPSULE, radius=0, length=0, position_offset=[0, 0, 0], orientation=[0, 0, 0, 1]): visual_shape = p.createVisualShape( shape, radius=radius, length=length, rgbaColor=[0.8, 0.6, 0.4, 1], specularColor=specular_color, visualFramePosition=position_offset, visualFrameOrientation=orientation, physicsClientId=self.id) collision_shape = p.createCollisionShape( shape, radius=radius, height=length, collisionFramePosition=position_offset, collisionFrameOrientation=orientation, physicsClientId=self.id) return collision_shape, visual_shape joint_c, joint_v = -1, -1 if gender == 'male': c = lambda tag: config(tag, 'human_male') m = c('mass') # mass of 50% male in kg rs = c('radius_scale') hs = c('height_scale') hs *= hmhs chest_c, chest_v = create_body( shape=p.GEOM_CAPSULE, radius=0.127 * rs, length=0.056, orientation=p.getQuaternionFromEuler([0, np.pi / 2.0, 0], physicsClientId=self.id)) right_shoulders_c, right_shoulders_v = create_body( shape=p.GEOM_CAPSULE, radius=0.106 * rs, length=0.253 / 8, position_offset=[-0.253 / 2.5 + 0.253 / 16, 0, 0], orientation=p.getQuaternionFromEuler([0, np.pi / 2.0, 0], physicsClientId=self.id)) left_shoulders_c, left_shoulders_v = create_body( shape=p.GEOM_CAPSULE, radius=0.106 * rs, length=0.253 / 8, position_offset=[0.253 / 2.5 - 0.253 / 16, 0, 0], orientation=p.getQuaternionFromEuler([0, np.pi / 2.0, 0], physicsClientId=self.id)) neck_c, neck_v = create_body( shape=p.GEOM_CAPSULE, radius=0.06 * rs, length=0.124 * hs, position_offset=[0, 0, (0.2565 - 0.1415 - 0.025) * hs]) upperarm_c, upperarm_v = create_body( shape=p.GEOM_CAPSULE, radius=0.043 * rs, length=0.279 * hs, position_offset=[0, 0, -0.279 / 2.0 * hs]) forearm_c, forearm_v = create_body( shape=p.GEOM_CAPSULE, radius=0.033 * rs, length=0.257 * hs, position_offset=[0, 0, -0.257 / 2.0 * hs]) hand_c, hand_v = create_body(shape=p.GEOM_SPHERE, radius=0.043 * rs, length=0, position_offset=[0, 0, -0.043 * rs]) self.hand_radius, self.elbow_radius, self.shoulder_radius = 0.043 * rs, 0.043 * rs, 0.043 * rs waist_c, waist_v = create_body( shape=p.GEOM_CAPSULE, radius=0.1205 * rs, length=0.049, orientation=p.getQuaternionFromEuler([0, np.pi / 2.0, 0], physicsClientId=self.id)) hips_c, hips_v = create_body(shape=p.GEOM_CAPSULE, radius=0.1335 * rs, length=0.094, position_offset=[0, 0, -0.08125 * hs], orientation=p.getQuaternionFromEuler( [0, np.pi / 2.0, 0], physicsClientId=self.id)) thigh_c, thigh_v = create_body( shape=p.GEOM_CAPSULE, radius=0.08 * rs, length=0.424 * hs, position_offset=[0, 0, -0.424 / 2.0 * hs]) shin_c, shin_v = create_body( shape=p.GEOM_CAPSULE, radius=0.05 * rs, length=0.403 * hs, position_offset=[0, 0, -0.403 / 2.0 * hs]) foot_c, foot_v = create_body( shape=p.GEOM_CAPSULE, radius=0.05 * rs, length=0.215 * hs, position_offset=[0, -0.1, -0.025 * rs], orientation=p.getQuaternionFromEuler([np.pi / 2.0, 0, 0], physicsClientId=self.id)) elbow_v = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=(0.043 + 0.033) / 2 * rs, length=0, rgbaColor=[0.8, 0.6, 0.4, 1], visualFramePosition=[0, 0.01, 0], physicsClientId=self.id) if self.cloth: # Cloth penetrates the spheres at the end of each capsule, so we create physical spheres at the joints invisible_v = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.01 * rs, length=0, rgbaColor=[0.8, 0.6, 0.4, 0], physicsClientId=self.id) shoulder_cloth_c, _ = create_body(shape=p.GEOM_SPHERE, radius=0.043 * rs, length=0) elbow_cloth_c, _ = create_body(shape=p.GEOM_SPHERE, radius=0.043 * rs, length=0) wrist_cloth_c, _ = create_body(shape=p.GEOM_SPHERE, radius=0.033 * rs, length=0) head_scale = [0.89] * 3 head_pos = [0.09, 0.08, -0.07 + 0.01] head_c = p.createCollisionShape( shapeType=p.GEOM_MESH, fileName=os.path.join( self.directory, 'head_female_male', 'BaseHeadMeshes_v5_male_cropped_reduced_compressed_vhacd.obj' ), collisionFramePosition=head_pos, collisionFrameOrientation=p.getQuaternionFromEuler( [np.pi / 2.0, 0, 0], physicsClientId=self.id), meshScale=head_scale, physicsClientId=self.id) head_v = p.createVisualShape( shapeType=p.GEOM_MESH, fileName=os.path.join( self.directory, 'head_female_male', 'BaseHeadMeshes_v5_male_cropped_reduced_compressed.obj'), rgbaColor=[0.8, 0.6, 0.4, 1], specularColor=specular_color, visualFramePosition=head_pos, visualFrameOrientation=p.getQuaternionFromEuler( [np.pi / 2.0, 0, 0], physicsClientId=self.id), meshScale=head_scale, physicsClientId=self.id) joint_p, joint_o = [0, 0, 0], [0, 0, 0, 1] chest_p = [0, 0, 0.156 * hs] shoulders_p = [0, 0, 0.1415 / 2 * hs] neck_p = [0, 0, 0.1515 * hs] head_p = [0, 0, (0.399 - 0.1415 - 0.1205) * hs] right_upperarm_p = [-0.106 * rs - 0.073, 0, 0] left_upperarm_p = [0.106 * rs + 0.073, 0, 0] forearm_p = [0, 0, -0.279 * hs] hand_p = [0, 0, -(0.033 * rs + 0.257 * hs)] waist_p = [0, 0, 0.08125 * hs] hips_p = [0, 0, 1.00825 * hs] right_thigh_p = [-0.08 * rs - 0.009, 0, -0.08125 * hs] left_thigh_p = [0.08 * rs + 0.009, 0, -0.08125 * hs] shin_p = [0, 0, -0.424 * hs] foot_p = [0, 0, -0.403 * hs - 0.025] else: c = lambda tag: config(tag, 'human_female') m = c('mass') # mass of 50% female in kg rs = c('radius_scale') hs = c('height_scale') hs *= hmhs chest_c, chest_v = create_body( shape=p.GEOM_CAPSULE, radius=0.127 * rs, length=0.01, orientation=p.getQuaternionFromEuler( [0, np.pi / 2.0, 0], physicsClientId=self.id)) # right_shoulders_c, right_shoulders_v = create_body( shape=p.GEOM_CAPSULE, radius=0.092 * rs, length=0.225 / 8, position_offset=[-0.225 / 2.5 + 0.225 / 16, 0, 0], orientation=p.getQuaternionFromEuler( [0, np.pi / 2.0, 0], physicsClientId=self.id)) # left_shoulders_c, left_shoulders_v = create_body( shape=p.GEOM_CAPSULE, radius=0.092 * rs, length=0.225 / 8, position_offset=[0.225 / 2.5 - 0.225 / 16, 0, 0], orientation=p.getQuaternionFromEuler( [0, np.pi / 2.0, 0], physicsClientId=self.id)) # neck_c, neck_v = create_body(shape=p.GEOM_CAPSULE, radius=0.05 * rs, length=0.121 * hs, position_offset=[ 0, 0, (0.2565 - 0.1415 - 0.025) * hs ]) # not position upperarm_c, upperarm_v = create_body( shape=p.GEOM_CAPSULE, radius=0.0355 * rs, length=0.264 * hs, position_offset=[0, 0, -0.264 / 2.0 * hs]) # forearm_c, forearm_v = create_body( shape=p.GEOM_CAPSULE, radius=0.027 * rs, length=0.234 * hs, position_offset=[0, 0, -0.234 / 2.0 * hs]) # hand_c, hand_v = create_body(shape=p.GEOM_SPHERE, radius=0.0355 * rs, length=0, position_offset=[0, 0, -0.0355 * rs]) # self.hand_radius, self.elbow_radius, self.shoulder_radius = 0.0355 * rs, 0.0355 * rs, 0.0355 * rs waist_c, waist_v = create_body( shape=p.GEOM_CAPSULE, radius=0.11 * rs, length=0.009, orientation=p.getQuaternionFromEuler( [0, np.pi / 2.0, 0], physicsClientId=self.id)) # hips_c, hips_v = create_body( shape=p.GEOM_CAPSULE, radius=0.127 * rs, length=0.117, position_offset=[0, 0, -0.15 / 2 * hs], orientation=p.getQuaternionFromEuler( [0, np.pi / 2.0, 0], physicsClientId=self.id)) # thigh_c, thigh_v = create_body( shape=p.GEOM_CAPSULE, radius=0.0775 * rs, length=0.391 * hs, position_offset=[0, 0, -0.391 / 2.0 * hs]) # shin_c, shin_v = create_body( shape=p.GEOM_CAPSULE, radius=0.045 * rs, length=0.367 * hs, position_offset=[0, 0, -0.367 / 2.0 * hs]) # foot_c, foot_v = create_body( shape=p.GEOM_CAPSULE, radius=0.045 * rs, length=0.195 * hs, position_offset=[0, -0.09, -0.0225 * rs], orientation=p.getQuaternionFromEuler( [np.pi / 2.0, 0, 0], physicsClientId=self.id)) # elbow_v = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=(0.0355 + 0.027) / 2 * rs, length=0, rgbaColor=[0.8, 0.6, 0.4, 1], visualFramePosition=[0, 0.01, 0], physicsClientId=self.id) if self.cloth: # Cloth penetrates the spheres at the end of each capsule, so we create physical spheres at the joints invisible_v = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.01 * rs, length=0, rgbaColor=[0.8, 0.6, 0.4, 0], physicsClientId=self.id) shoulder_cloth_c, _ = create_body(shape=p.GEOM_SPHERE, radius=0.0355 * rs, length=0) elbow_cloth_c, _ = create_body(shape=p.GEOM_SPHERE, radius=0.0355 * rs, length=0) wrist_cloth_c, _ = create_body(shape=p.GEOM_SPHERE, radius=0.027 * rs, length=0) head_scale = [0.89] * 3 head_pos = [-0.089, -0.09, -0.07] head_c = p.createCollisionShape( shapeType=p.GEOM_MESH, fileName=os.path.join( self.directory, 'head_female_male', 'BaseHeadMeshes_v5_female_cropped_reduced_compressed_vhacd.obj' ), collisionFramePosition=head_pos, collisionFrameOrientation=p.getQuaternionFromEuler( [np.pi / 2.0, 0, 0], physicsClientId=self.id), meshScale=head_scale, physicsClientId=self.id) head_v = p.createVisualShape( shapeType=p.GEOM_MESH, fileName=os.path.join( self.directory, 'head_female_male', 'BaseHeadMeshes_v5_female_cropped_reduced_compressed.obj'), rgbaColor=[0.8, 0.6, 0.4, 1], specularColor=specular_color, visualFramePosition=head_pos, visualFrameOrientation=p.getQuaternionFromEuler( [np.pi / 2.0, 0, 0], physicsClientId=self.id), meshScale=head_scale, physicsClientId=self.id) joint_p, joint_o = [0, 0, 0], [0, 0, 0, 1] chest_p = [0, 0, 0.15 * hs] shoulders_p = [0, 0, 0.132 / 2 * hs] neck_p = [0, 0, 0.132 * hs] head_p = [0, 0, 0.12 * hs] right_upperarm_p = [-0.092 * rs - 0.067, 0, 0] left_upperarm_p = [0.092 * rs + 0.067, 0, 0] forearm_p = [0, 0, -0.264 * hs] hand_p = [0, 0, -(0.027 * rs + 0.234 * hs)] waist_p = [0, 0, 0.15 / 2 * hs] hips_p = [0, 0, 0.923 * hs] right_thigh_p = [-0.0775 * rs - 0.0145, 0, -0.15 / 2 * hs] left_thigh_p = [0.0775 * rs + 0.0145, 0, -0.15 / 2 * hs] shin_p = [0, 0, -0.391 * hs] foot_p = [0, 0, -0.367 * hs - 0.045 / 2] linkMasses = [] linkCollisionShapeIndices = [] linkVisualShapeIndices = [] linkPositions = [] linkOrientations = [] linkInertialFramePositions = [] linkInertialFrameOrientations = [] linkParentIndices = [] linkJointTypes = [] linkJointAxis = [] linkLowerLimits = [] linkUpperLimits = [] # NOTE: Waist and chest linkMasses.extend(m * np.array([0, 0, 0.13, 0.1])) linkCollisionShapeIndices.extend([joint_c, joint_c, waist_c, chest_c]) linkVisualShapeIndices.extend([joint_v, joint_v, waist_v, chest_v]) linkPositions.extend([waist_p, joint_p, joint_p, chest_p]) linkOrientations.extend([joint_o] * 4) linkInertialFramePositions.extend([[0, 0, 0]] * 4) linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 4) linkParentIndices.extend([0, 1, 2, 3]) if new: linkJointTypes.extend([p.JOINT_REVOLUTE] * 3 + [p.JOINT_FIXED]) linkJointAxis.extend([[1, 0, 0], [0, 1, 0], [0, 0, 1], [0, 0, 0]]) linkLowerLimits.extend( np.array( [np.deg2rad(-30), np.deg2rad(-30), np.deg2rad(-30), 0])) linkUpperLimits.extend( np.array([np.deg2rad(75), np.deg2rad(30), np.deg2rad(30), 0])) else: linkJointTypes.extend([p.JOINT_FIXED] * 4) linkJointAxis.extend([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]) linkLowerLimits.extend(np.array([0, 0, 0, 0])) linkUpperLimits.extend(np.array([0, 0, 0, 0])) # NOTE: Shoulders, neck, and head linkMasses.extend(m * np.array([0, 0, 0.05, 0, 0, 0.05, 0.01, 0, 0, 0.07])) linkCollisionShapeIndices.extend([ joint_c, joint_c, right_shoulders_c, joint_c, joint_c, left_shoulders_c, neck_c, joint_c, joint_c, head_c ]) linkVisualShapeIndices.extend([ joint_v, joint_v, right_shoulders_v, joint_v, joint_v, left_shoulders_v, neck_v, joint_v, joint_v, head_v ]) linkPositions.extend([ shoulders_p, shoulders_p, joint_p, shoulders_p, shoulders_p, joint_p, neck_p, head_p, joint_p, joint_p ]) linkOrientations.extend([joint_o] * 10) linkInertialFramePositions.extend([[0, 0, 0]] * 10) linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 10) linkParentIndices.extend([4, 5, 6, 4, 8, 9, 4, 11, 12, 13]) linkJointTypes.extend([p.JOINT_FIXED] * 6 + [p.JOINT_REVOLUTE] * 4) linkJointAxis.extend([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [1, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]]) linkLowerLimits.extend( np.array([ 0, 0, 0, 0, 0, 0, np.deg2rad(-10), np.deg2rad(-50), np.deg2rad(-34), np.deg2rad(-70) ]) * limit_scale) linkUpperLimits.extend( np.array([ 0, 0, 0, 0, 0, 0, np.deg2rad(20), np.deg2rad(50), np.deg2rad(34), np.deg2rad(70) ]) * limit_scale) # NOTE: Right arm linkMasses.extend(m * np.array([0, 0, 0.033, 0, 0.019, 0, 0.0065])) if not self.cloth: linkCollisionShapeIndices.extend([ joint_c, joint_c, upperarm_c, joint_c, forearm_c, joint_c, hand_c ]) linkVisualShapeIndices.extend([ joint_v, joint_v, upperarm_v, elbow_v, forearm_v, joint_v, hand_v ]) else: linkCollisionShapeIndices.extend([ joint_c, shoulder_cloth_c, upperarm_c, elbow_cloth_c, forearm_c, wrist_cloth_c, hand_c ]) linkVisualShapeIndices.extend([ joint_v, invisible_v, upperarm_v, elbow_v, forearm_v, invisible_v, hand_v ]) linkPositions.extend([ right_upperarm_p, joint_p, joint_p, forearm_p, joint_p, hand_p, joint_p ]) linkOrientations.extend([joint_o] * 7) linkInertialFramePositions.extend([[0, 0, 0]] * 7) linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 7) linkParentIndices.extend([7, 15, 16, 17, 18, 19, 20]) linkJointTypes.extend([p.JOINT_REVOLUTE] * 7) linkJointAxis.extend([[0, 1, 0], [1, 0, 0], [0, 0, 1], [1, 0, 0], [0, 0, 1], [1, 0, 0], [0, 1, 0]]) linkLowerLimits.extend( np.array([ np.deg2rad(5), np.deg2rad(-188), np.deg2rad(-90), np.deg2rad(-128), np.deg2rad(-90), np.deg2rad(-81), np.deg2rad(-27) ]) * limit_scale) linkUpperLimits.extend( np.array([ np.deg2rad(198), np.deg2rad(61), np.deg2rad(90), np.deg2rad(0), np.deg2rad(90), np.deg2rad(90), np.deg2rad(47) ]) * limit_scale) # NOTE: Left arm linkMasses.extend(m * np.array([0, 0, 0.033, 0, 0.019, 0, 0.0065])) if not self.cloth: linkCollisionShapeIndices.extend([ joint_c, joint_c, upperarm_c, joint_c, forearm_c, joint_c, hand_c ]) linkVisualShapeIndices.extend([ joint_v, joint_v, upperarm_v, elbow_v, forearm_v, joint_v, hand_v ]) else: linkCollisionShapeIndices.extend([ joint_c, shoulder_cloth_c, upperarm_c, elbow_cloth_c, forearm_c, wrist_cloth_c, hand_c ]) linkVisualShapeIndices.extend([ joint_v, invisible_v, upperarm_v, elbow_v, forearm_v, invisible_v, hand_v ]) linkPositions.extend([ left_upperarm_p, joint_p, joint_p, forearm_p, joint_p, hand_p, joint_p ]) linkOrientations.extend([joint_o] * 7) linkInertialFramePositions.extend([[0, 0, 0]] * 7) linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 7) linkParentIndices.extend([10, 22, 23, 24, 25, 26, 27]) linkJointTypes.extend([p.JOINT_REVOLUTE] * 7) linkJointAxis.extend([[0, 1, 0], [1, 0, 0], [0, 0, 1], [1, 0, 0], [0, 0, 1], [1, 0, 0], [0, 1, 0]]) linkLowerLimits.extend( np.array([ np.deg2rad(-198), np.deg2rad(-188), np.deg2rad(-90), np.deg2rad(-128), np.deg2rad(-90), np.deg2rad(-81), np.deg2rad(-47) ]) * limit_scale) linkUpperLimits.extend( np.array([ np.deg2rad(-5), np.deg2rad(61), np.deg2rad(90), np.deg2rad(0), np.deg2rad(90), np.deg2rad(90), np.deg2rad(27) ]) * limit_scale) # NOTE: Right leg linkMasses.extend(m * np.array([0, 0, 0.105, 0.0475, 0, 0, 0.014])) linkCollisionShapeIndices.extend( [joint_c, joint_c, thigh_c, shin_c, joint_c, joint_c, foot_c]) linkVisualShapeIndices.extend( [joint_v, joint_v, thigh_v, shin_v, joint_v, joint_v, foot_v]) linkPositions.extend([ right_thigh_p, joint_p, joint_p, shin_p, foot_p, joint_p, joint_p ]) linkOrientations.extend([joint_o] * 7) linkInertialFramePositions.extend([[0, 0, 0]] * 7) linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 7) linkParentIndices.extend([0, 29, 30, 31, 32, 33, 34]) linkJointTypes.extend([p.JOINT_REVOLUTE] * 7) linkJointAxis.extend([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]]) linkLowerLimits.extend( np.array([ np.deg2rad(-127), np.deg2rad(-40), np.deg2rad(-45), 0, np.deg2rad(-35), np.deg2rad(-23), np.deg2rad(-43) ])) linkUpperLimits.extend( np.array([ np.deg2rad(30), np.deg2rad(45), np.deg2rad(40), np.deg2rad(130), np.deg2rad(38), np.deg2rad(24), np.deg2rad(35) ])) # NOTE: Left leg linkMasses.extend(m * np.array([0, 0, 0.105, 0.0475, 0, 0, 0.014])) linkCollisionShapeIndices.extend( [joint_c, joint_c, thigh_c, shin_c, joint_c, joint_c, foot_c]) linkVisualShapeIndices.extend( [joint_v, joint_v, thigh_v, shin_v, joint_v, joint_v, foot_v]) linkPositions.extend( [left_thigh_p, joint_p, joint_p, shin_p, foot_p, joint_p, joint_p]) linkOrientations.extend([joint_o] * 7) linkInertialFramePositions.extend([[0, 0, 0]] * 7) linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 7) linkParentIndices.extend([0, 36, 37, 38, 39, 40, 41]) linkJointTypes.extend([p.JOINT_REVOLUTE] * 7) linkJointAxis.extend([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]]) linkLowerLimits.extend( np.array([ np.deg2rad(-127), np.deg2rad(-45), np.deg2rad(-40), 0, np.deg2rad(-35), np.deg2rad(-24), np.deg2rad(-35) ])) linkUpperLimits.extend( np.array([ np.deg2rad(30), np.deg2rad(40), np.deg2rad(45), np.deg2rad(130), np.deg2rad(38), np.deg2rad(23), np.deg2rad(43) ])) human = p.createMultiBody( baseMass=0 if static else m * 0.14, baseCollisionShapeIndex=hips_c, baseVisualShapeIndex=hips_v, basePosition=[10, 10, 10], baseOrientation=[0, 0, 0, 1], linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=linkParentIndices, linkJointTypes=linkJointTypes, linkJointAxis=linkJointAxis, linkLowerLimits=linkLowerLimits, linkUpperLimits=linkUpperLimits, useMaximalCoordinates=False, flags=p.URDF_USE_SELF_COLLISION, physicsClientId=self.id) # Self collision has been enabled for the person # For stability: Remove all collisions except between the arms/legs and the other body parts num_joints = p.getNumJoints(human, physicsClientId=self.id) for i in range(-1, num_joints): for j in range(-1, num_joints): p.setCollisionFilterPair(human, human, i, j, 0, physicsClientId=self.id) for i in range(7, 14): # Right arm for j in list(range(-1, 4)) + list(range(14, num_joints)): p.setCollisionFilterPair(human, human, i, j, 1, physicsClientId=self.id) for i in range(17, 24): # Left arm for j in list(range(-1, 14)) + list(range(24, num_joints)): p.setCollisionFilterPair(human, human, i, j, 1, physicsClientId=self.id) for i in range(28, 35): # Right leg for j in [-1] + list(range(4, 28)) + list(range(35, num_joints)): p.setCollisionFilterPair(human, human, i, j, 1, physicsClientId=self.id) for i in range(35, num_joints): # Left leg for j in [-1] + list(range(4, 35)): p.setCollisionFilterPair(human, human, i, j, 1, physicsClientId=self.id) human_joint_states = p.getJointStates( human, jointIndices=list( range(p.getNumJoints(human, physicsClientId=self.id))), physicsClientId=self.id) human_joint_positions = np.array([x[0] for x in human_joint_states]) for j in range(p.getNumJoints(human, physicsClientId=self.id)): p.resetJointState(human, jointIndex=j, targetValue=0, targetVelocity=0, physicsClientId=self.id) return human
def train_adversarial(): leg_model_nn = LegModelLSTM(n_inputs=6, n_actions=3) act_gen_nn = LegModelLSTM(n_inputs=3, n_actions=3, use_tanh=True) optim_disc = T.optim.Adam(leg_model_nn.parameters(), lr=0.002) optim_gen = T.optim.Adam(act_gen_nn.parameters(), lr=0.004) print("Training:") joints = [] acts = [] batch_rnd_noises = [] for iter in range(n_iters): # Sample noise vec from generator rnd_noise = np.random.rand(episode_len, 3).astype(np.float32) * 2. - 1. batch_rnd_noises.append(rnd_noise) with T.no_grad(): act_noise, _ = act_gen_nn(T.tensor(rnd_noise).unsqueeze(0)) ep_joints = [] ep_acts = [] # Sample batchsize episodes [ p.resetJointState(leg, i, np.random.rand() * 2 - 1, 0, physicsClientId=client_ID) for i in range(3) ] for st in range(episode_len): # Joint obs = p.getJointStates(leg, range(3), physicsClientId=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]) joint_angles_normed = scale_joints(joint_angles) ep_joints.append(joint_angles_normed) # Action act = act_noise[0, st] #if np.random.rand() < 0.001: print(act) action_rads = scale_action(act) ep_acts.append(act) # Simulate for i in range(3): p.setJointMotorControl2(bodyUniqueId=leg, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=action_rads[i], force=max_joint_force, positionGain=0.1, velocityGain=0.1, maxVelocity=2.0, physicsClientId=client_ID) for i in range(sim_steps_per_iter): p.stepSimulation(physicsClientId=client_ID) if (animate) and True: time.sleep(0.0038) joints.append(ep_joints) acts.append(ep_acts) if iter % batchsize == 0 and iter > 0: # Forward pass joints_T = T.tensor([j[:-1] for j in joints], dtype=T.float32) joints_T_gen = joints_T.clone() joints_next_T = T.tensor([j[1:] for j in joints], dtype=T.float32) joints_next_T_gen = joints_next_T.clone() act_noises_T = T.tensor([j[:-1] for j in batch_rnd_noises]) acts_T, _ = act_gen_nn(act_noises_T) pred, _ = leg_model_nn(T.cat((joints_T, acts_T), dim=2)) if iter % batchsize * 3 == 0: # Do discriminator first with T.no_grad(): acts_T, _ = act_gen_nn(act_noises_T) pred, _ = leg_model_nn(T.cat((joints_T_gen, acts_T), dim=2)) optim_disc.zero_grad() loss_disc = F.mse_loss(pred, joints_next_T_gen) loss_disc.backward() optim_disc.step() # Update gen acts_T, _ = act_gen_nn(act_noises_T) pred, _ = leg_model_nn(T.cat((joints_T, acts_T), dim=2)) optim_gen.zero_grad() loss_gen = -F.mse_loss(pred, joints_next_T) loss_gen.backward() optim_gen.step() print("Iter: {}/{}, loss: {}".format(iter, n_iters, loss_disc)) joints = [] acts = [] batch_rnd_noises = [] sdir_disc = os.path.join(os.path.dirname(os.path.realpath(__file__)), "legtester.p") sdir_gen = os.path.join(os.path.dirname(os.path.realpath(__file__)), "legtester_gen.p") T.save(leg_model_nn.state_dict(), sdir_disc) T.save(leg_model_nn.state_dict(), sdir_gen) print("Training done")
def reset(self): random_delta = np.random.random(len( self.joint_ids)) * self.random_reset for joint_id, angle, d_angle in zip(self.joint_ids, self.rest_position, random_delta): p.resetJointState(self.uid, joint_id, angle + d_angle)
import pybullet as p import time cid = p.connect(p.SHARED_MEMORY) if (cid<0): p.connect(p.GUI) p.loadURDF("plane.urdf") kuka = p.loadURDF("kuka_iiwa/model.urdf") p.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textSize=1.5,parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugLine([0,0,0],[0.1,0,0],[1,0,0],parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugLine([0,0,0],[0,0.1,0],[0,1,0],parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],parentObjectUniqueId=kuka, parentLinkIndex=6) p.setRealTimeSimulation(0) angle=0 while (True): time.sleep(0.01) p.resetJointState(kuka,2,angle) p.resetJointState(kuka,3,angle) angle+=0.01
p.setAdditionalSearchPath(pybullet_data.getDataPath()) if (clid < 0): p.connect(p.GUI) p.loadURDF("plane.urdf", [0, 0, -0.3]) husky = p.loadURDF("husky/husky.urdf", [0.290388, 0.329902, -0.310270], [0.002328, -0.000984, 0.996491, 0.083659]) for i in range(p.getNumJoints(husky)): print(p.getJointInfo(husky, i)) kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749, 0.345564, 0.120208, 0.002327, -0.000988, 0.996491, 0.083659) ob = kukaId jointPositions = [ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ] for jointIndex in range(p.getNumJoints(ob)): p.resetJointState(ob, jointIndex, jointPositions[jointIndex]) #put kuka on top of husky cid = p.createConstraint(husky, -1, kukaId, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0., 0., -.5], [0, 0, 0, 1]) baseorn = p.getQuaternionFromEuler([3.1415, 0, 0.3]) baseorn = [0, 0, 0, 1] #[0, 0, 0.707, 0.707] #p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1]) kukaEndEffectorIndex = 6 numJoints = p.getNumJoints(kukaId) if (numJoints != 7): exit()
if (useRealTimeSimulation): dt = datetime.now() t = (dt.second / 60.) * 2. * math.pi else: t = t + 0.01 time.sleep(0.01) for i in range(1): pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)] jointPoses = p.calculateInverseKinematics(sawyerId, sawyerEndEffectorIndex, pos, jointDamping=jd, solver=ikSolver, maxNumIterations=100) #reset the joint state (ignoring all dynamics, not recommended to use during simulation) for i in range(numJoints): jointInfo = p.getJointInfo(sawyerId, i) qIndex = jointInfo[3] if qIndex > -1: p.resetJointState(sawyerId, i, jointPoses[qIndex - 7]) ls = p.getLinkState(sawyerId, sawyerEndEffectorIndex) if (hasPrevPose): p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration) p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration) prevPose = pos prevPose1 = ls[4] hasPrevPose = 1
record.append(values[i]) log.append(record) return log #clid = p.connect(p.SHARED_MEMORY) p.connect(p.GUI) p.loadURDF("plane.urdf",[0,0,-0.3]) p.loadURDF("kuka_iiwa/model.urdf",[0,0,0]) p.loadURDF("cube.urdf",[2,2,5]) p.loadURDF("cube.urdf",[-2,-2,5]) p.loadURDF("cube.urdf",[2,-2,5]) log = readLogFile("LOG0001.txt") recordNum = len(log) itemNum = len(log[0]) print('record num:'), print(recordNum) print('item num:'), print(itemNum) for record in log: Id = record[1] pos = [record[2],record[3],record[4]] orn = [record[5],record[6],record[7],record[8]] p.resetBasePositionAndOrientation(Id,pos,orn) numJoints = record[15] for i in range (numJoints): p.resetJointState(Id,i,record[i+16]) sleep(0.0005)
import pybullet as p import time import math p.connect(p.SHARED_MEMORY) p.loadURDF("plane.urdf") quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3) #p.getNumJoints(1) #right front leg p.resetJointState(quadruped,0,1.57) p.resetJointState(quadruped,2,-2.2) p.resetJointState(quadruped,3,-1.57) p.resetJointState(quadruped,5,2.2) p.createConstraint(quadruped,2,quadruped,5,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,1.57,1) p.setJointMotorControl(quadruped,1,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,2,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1) p.setJointMotorControl(quadruped,4,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,5,p.VELOCITY_CONTROL,0,0) #left front leg p.resetJointState(quadruped,6,1.57) p.resetJointState(quadruped,8,-2.2) p.resetJointState(quadruped,9,-1.57) p.resetJointState(quadruped,11,2.2) p.createConstraint(quadruped,8,quadruped,11,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,1.57,1)
def test_rnd(param_sigma=.0): print("Loading model and testing") sdir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "legtester.p") leg_model_nn = LegModelLSTM(n_inputs=6, n_actions=3) leg_model_nn.load_state_dict(T.load(sdir)) ou = OrnsteinUhlenbeckActionNoise(mu=np.zeros(3), sigma=0.5, theta=0.15, dt=1e-1, x0=None) # Set seed np.random.seed(1337) n_test_iters = batchsize * 5 # Test joints = [] acts = [] losses = [] for iter in range(n_test_iters): max_velocity = 2.0 - np.random.rand() * param_sigma ou.reset() correlated_noise = [ou() for i in range(episode_len)] ep_joints = [] ep_acts = [] # Sample batchsize episodes [ p.resetJointState(leg, i, np.random.randn(), 0, physicsClientId=client_ID) for i in range(3) ] for st in range(episode_len): # Joint obs = p.getJointStates(leg, range(3), physicsClientId=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]) joint_angles_normed = scale_joints(joint_angles) ep_joints.append(joint_angles_normed) # Action # act_normed = np.clip(np.random.randn(3), -1, 1) act_normed = np.clip(correlated_noise[st], -1, 1) scaled_action = scale_action(act_normed) ep_acts.append(act_normed) # Simulate for i in range(3): p.setJointMotorControl2( bodyUniqueId=leg, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=scaled_action[i], force=max_joint_force, positionGain=0.1, # 0.1 velocityGain=0.1, # 0.1 maxVelocity=max_velocity, # 2.0 physicsClientId=client_ID) for i in range(sim_steps_per_iter): p.stepSimulation(physicsClientId=client_ID) if (animate) and True: time.sleep(0.0038) joints.append(ep_joints) acts.append(ep_acts) if iter % batchsize == 0 and iter > 0: # Forward pass joints_T = T.tensor([j[:-1] for j in joints], dtype=T.float32) joints_next_T = T.tensor([j[1:] for j in joints], dtype=T.float32) acts_T = T.tensor([j[:-1] for j in acts], dtype=T.float32) pred, _ = leg_model_nn(T.cat((joints_T, acts_T), dim=2)) joints = [] acts = [] # update loss = F.mse_loss(pred, joints_next_T) losses.append(loss.data) print("Iter: {}/{}, loss: {}".format(iter, n_test_iters, loss)) print("Mean loss: {}".format(np.mean(losses)))
print("loaded!") #p.changeDynamics(sphere ,-1, mass=1000) door = p.loadURDF("door.urdf",[0,0,-11]) p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1) print("numJoints = ", p.getNumJoints(door)) p.setGravity(0,0,-10) position_control = True angle = math.pi*0.25 p.resetJointState(door,1,angle) angleread = p.getJointState(door,1) print("angleread = ",angleread) prevTime = time.time() angle = math.pi*0.5 count=0 while (1): count+=1 if (count==12): p.stopStateLogging(logId) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) curTime = time.time()
def loadRobot(self, translation, quaternion, physicsClientId=0): """ Overloads @loadRobot from the @RobotVirtual class, loads the robot into the simulated instance. This method also updates the max velocity of the robot's fingers, adds self collision filters to the model and defines the cameras of the model in the camera_dict. Parameters: translation - List containing 3 elements, the translation [x, y, z] of the robot in the WORLD frame quaternion - List containing 4 elements, the quaternion [x, y, z, q] of the robot in the WORLD frame physicsClientId - The id of the simulated instance in which the robot is supposed to be loaded """ pybullet.setAdditionalSearchPath(os.path.dirname( os.path.realpath(__file__)), physicsClientId=physicsClientId) # Add 0.50 meters on the z component, avoing to spawn NAO in the ground translation = [translation[0], translation[1], translation[2] + 1.05] RobotVirtual.loadRobot(self, translation, quaternion, physicsClientId=physicsClientId) balance_constraint = pybullet.createConstraint( parentBodyUniqueId=self.robot_model, parentLinkIndex=-1, childBodyUniqueId=-1, childLinkIndex=-1, jointType=pybullet.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=[0, 0, 0], parentFrameOrientation=[0, 0, 0, 1], childFramePosition=translation, childFrameOrientation=quaternion, physicsClientId=self.physics_client) self.goToPosture("Stand", 1.0) pybullet.setCollisionFilterPair(self.robot_model, self.robot_model, self.link_dict["REye"].getIndex(), self.link_dict["LEye"].getIndex(), 0, physicsClientId=self.physics_client) for link in ["torso", "HeadRoll_link"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict["NeckPitch_link"].getIndex(), self.link_dict[link].getIndex(), 0, physicsClientId=self.physics_client) for side in ["R", "L"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Eye"].getIndex(), self.link_dict["HeadRoll_link"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Thigh"].getIndex(), self.link_dict["body"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side.lower() + "_ankle"].getIndex(), self.link_dict[side + "Tibia"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "ShoulderYaw_link"].getIndex(), self.link_dict["torso"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "WristRoll_link"].getIndex(), self.link_dict[side.lower() + "_wrist"].getIndex(), 0, physicsClientId=self.physics_client) for link in ["ShoulderYaw_link", "WristYaw_link"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + link].getIndex(), self.link_dict[side + "Elbow"].getIndex(), 0, physicsClientId=self.physics_client) for name, link in self.link_dict.items(): if side + "Finger" in name or\ side + "Thumb" in name: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side.lower() + "_wrist"].getIndex(), link.getIndex(), 0, physicsClientId=self.physics_client) for joint in self.joint_dict.values(): pybullet.resetJointState(self.robot_model, joint.getIndex(), 0.0, physicsClientId=self.physics_client) pybullet.removeConstraint(balance_constraint, physicsClientId=self.physics_client) for joint_name in list(self.joint_dict): if 'RFinger' in joint_name or 'RThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["RHand"].getMaxVelocity()) elif 'LFinger' in joint_name or 'LThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["LHand"].getMaxVelocity()) camera_right = CameraRgb( self.robot_model, RomeoVirtual.ID_CAMERA_RIGHT, self.link_dict["CameraRightEye_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) camera_left = CameraRgb(self.robot_model, RomeoVirtual.ID_CAMERA_LEFT, self.link_dict["CameraLeftEye_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) camera_depth = CameraDepth(self.robot_model, RomeoVirtual.ID_CAMERA_DEPTH, self.link_dict["CameraDepth_optical_frame"], hfov=58.0, vfov=45.0, physicsClientId=self.physics_client) self.camera_dict = { RomeoVirtual.ID_CAMERA_RIGHT: camera_right, RomeoVirtual.ID_CAMERA_LEFT: camera_left, RomeoVirtual.ID_CAMERA_DEPTH: camera_depth } # The frequency of the IMU is set to 100Hz self.imu = Imu(self.robot_model, self.link_dict["torso"], 100.0, physicsClientId=self.physics_client)
def applyAction(self, motorCommands): #print ("self.numJoints") #print (self.numJoints) if (self.useInverseKinematics): dx = motorCommands[0] dy = motorCommands[1] dz = motorCommands[2] da = motorCommands[3] fingerAngle = motorCommands[4] state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex) actualEndEffectorPos = state[0] #print("pos[2] (getLinkState(kukaEndEffectorIndex)") #print(actualEndEffectorPos[2]) self.endEffectorPos[0] = self.endEffectorPos[0]+dx if (self.endEffectorPos[0]>0.65): self.endEffectorPos[0]=0.65 if (self.endEffectorPos[0]<0.50): self.endEffectorPos[0]=0.50 self.endEffectorPos[1] = self.endEffectorPos[1]+dy if (self.endEffectorPos[1]<-0.17): self.endEffectorPos[1]=-0.17 if (self.endEffectorPos[1]>0.22): self.endEffectorPos[1]=0.22 #print ("self.endEffectorPos[2]") #print (self.endEffectorPos[2]) #print("actualEndEffectorPos[2]") #print(actualEndEffectorPos[2]) #if (dz<0 or actualEndEffectorPos[2]<0.5): self.endEffectorPos[2] = self.endEffectorPos[2]+dz self.endEffectorAngle = self.endEffectorAngle + da pos = self.endEffectorPos orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw]) if (self.useNullSpace==1): if (self.useOrientation==1): jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp) else: jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp) else: if (self.useOrientation==1): jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd) else: jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos) #print("jointPoses") #print(jointPoses) #print("self.kukaEndEffectorIndex") #print(self.kukaEndEffectorIndex) if (self.useSimulation): for i in range (self.kukaEndEffectorIndex+1): #print(i) p.setJointMotorControl2(bodyUniqueId=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,maxVelocity=self.maxVelocity, positionGain=0.3,velocityGain=1) else: #reset the joint state (ignoring all dynamics, not recommended to use during simulation) for i in range (self.numJoints): p.resetJointState(self.kukaUid,i,jointPoses[i]) #fingers p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce) p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce) p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce) p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce) p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce) else: for action in range (len(motorCommands)): motor = self.motorIndices[action] p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
def reset_position(self, position, velocity): p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, targetValue=position, targetVelocity=velocity) self.disable_motor()
objects = [ p.loadURDF("plane.urdf", 0.000000, 0.000000, -.300000, 0.000000, 0.000000, 0.000000, 1.000000) ] objects = [ p.loadURDF("quadruped/minitaur.urdf", [-0.000046, -0.000068, 0.200774], [-0.000701, 0.000387, -0.000252, 1.000000], useFixedBase=False) ] ob = objects[0] jointPositions = [ 0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000, -2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193, 0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517 ] for ji in range(p.getNumJoints(ob)): p.resetJointState(ob, ji, jointPositions[ji]) p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0) cid0 = p.createConstraint(1, 3, 1, 6, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000], [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000], [0.000000, 0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 0.000000, 1.000000]) p.changeConstraint(cid0, maxForce=500.000000) cid1 = p.createConstraint(1, 16, 1, 19, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000], [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000], [0.000000, 0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 0.000000, 1.000000]) p.changeConstraint(cid1, maxForce=500.000000) cid2 = p.createConstraint(1, 9, 1, 12, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000], [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000], [0.000000, 0.000000, 0.000000, 1.000000],
import pybullet as p import pybullet_data import utils useMaximalCoordinates = False p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates) for i in range(2): p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=500) for i in range(2): p.resetJointState(pole, i, 0.3, targetVelocity=0.3) timeStep = 0.0000001 p.setTimeStep(timeStep) while p.isConnected(): position, velocity, _, _ = p.getJointState(pole, 0) print(velocity) p.stepSimulation()
rightLegAngles= ik.rl_com_from_foot(com_0 , orientation) leftLegAngles = ik.ll_com_from_foot(com_0 , orientation) for i, index in enumerate(rightLegIndices): if (not np.isnan(rightLegAngles[i])) and (rightLegAngles[i] > p.getJointInfo(teoId, index)[8] and rightLegAngles[i] < p.getJointInfo(teoId, index)[9]): targetPos[index]=rightLegAngles[i] for i, index in enumerate(leftLegIndices): if (not np.isnan(leftLegAngles[i])) and ( leftLegAngles[i] > p.getJointInfo(teoId, index)[8] and leftLegAngles[i] < p.getJointInfo(teoId, index)[9]): targetPos[index]=leftLegAngles[i] for jointIndex in range(n_joints): p.resetJointState(teoId, jointIndex, targetPos[jointIndex]) mode = p.POSITION_CONTROL p.setJointMotorControlArray(teoId, jointIndices, controlMode=mode, forces=maxForces, targetPositions = targetPos ) offset_torque = maxForces[name2id[b'l_hip_roll']] max_offset = radians(1.2581265067842706) min_offset = radians(0.7139893352971366) print("max_offset:", max_offset)
cid = p.connect(p.SHARED_MEMORY) if (cid<0): p.connect(p.GUI) p.resetSimulation() #disable rendering during loading makes it much faster p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)] pr2_gripper = objects[0] print ("pr2_gripper=") print (pr2_gripper) jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ] for jointIndex in range (p.getNumJoints(pr2_gripper)): p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex]) pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000]) print ("pr2_cid") print (pr2_cid) objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)] kuka = objects[0] jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ] for jointIndex in range (p.getNumJoints(kuka)): p.resetJointState(kuka,jointIndex,jointPositions[jointIndex]) p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
import pybullet as p import time p.connect(p.GUI) p.loadURDF("plane.urdf", [0, 0, -0.25]) minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True) print(p.getNumJoints(minitaur)) p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=23.2, cameraPitch=-6.6, cameraTargetPosition=[-0.064, .621, -0.2]) motorJointId = 1 p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0) p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1) angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0) jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0) textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2]) p.setRealTimeSimulation(1) while (1): frictionForce = p.readUserDebugParameter(jointFrictionForceSlider) angularDamping = p.readUserDebugParameter(angularDampingSlider) p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=0, force=frictionForce) p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping) time.sleep(0.01)
applyMMMRotationToURDFJoint(human_adult_ID, 1, JProcessed[t, 3], JProcessed[t, 4], JProcessed[t, 5], inverse=True) # pelvis to right leg applyMMMRotationToURDFJoint(human_adult_ID, 2, JProcessed[t, 33], JProcessed[t, 34], JProcessed[t, 35]) # pelvis to left leg applyMMMRotationToURDFJoint(human_adult_ID, 3, JProcessed[t, 17], JProcessed[t, 18], JProcessed[t, 19]) # right leg to right shin p.resetJointState(human_adult_ID, 4, -JProcessed[t, 36]) # left leg to left shin p.resetJointState(human_adult_ID, 5, -JProcessed[t, 20]) # right shin to right foot applyMMMRotationToURDFJoint(human_adult_ID, 6, JProcessed[t, 28], JProcessed[t, 29], JProcessed[t, 30]) # left shin to left foot applyMMMRotationToURDFJoint(human_adult_ID, 7, JProcessed[t, 12], JProcessed[t, 13], JProcessed[t, 14]) # chest_to_right_arm applyMMMRotationToURDFJoint(human_adult_ID, 8, JProcessed[t, 37], JProcessed[t, 38], JProcessed[t, 39]) # chest_to_left_arm
import pybullet as p import time p.connect(p.DIRECT) p.setGravity(0,0,-10) p.setPhysicsEngineParameter(numSolverIterations=5) p.setPhysicsEngineParameter(fixedTimeStep=1./240.) p.setPhysicsEngineParameter(numSubSteps=1) objects = p.loadMJCF("mjcf/humanoid_symmetric.xml") ob = objects[0] p.resetBasePositionAndOrientation(ob,[0.000000,0.000000,0.000000],[0.000000,0.000000,0.000000,1.000000]) ob = objects[1] p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923]) jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ] for jointIndex in range (p.getNumJoints(ob)): p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) #first let the humanoid fall #p.setRealTimeSimulation(1) #time.sleep(5) p.setRealTimeSimulation(0) #p.saveWorld("lyiing.py") #now do a benchmark print("Starting benchmark") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"pybullet_humanoid_timings.json") for i in range(1000): p.stepSimulation() p.stopStateLogging(logId)
def reset_sim(com_0 = np.array([0,0,l9+l10+l11+l12-0.001]), orientation=np.array([0,0,0]), g=-9.79983): p.resetSimulation() p.setTimeStep(timestep) p.setGravity(0,0,g) planeId = p.loadURDF("plane.urdf") teoStartOrientation = p.getQuaternionFromEuler(-orientation) teoStartPosition = [0,0,com_0[2]] urdf_path = os.path.join(urdf_root,"TEO_wobble.urdf") teoId = p.loadURDF(urdf_path,teoStartPosition ,teoStartOrientation) #print("TEO pos =",p.getBasePositionAndOrientation(teoId)[0]) n_joints = p.getNumJoints(teoId) name2id={} for i in range(n_joints): name2id[p.getJointInfo(teoId,i)[1]]=i n_joints = p.getNumJoints(teoId) jointIndices = [0] * n_joints targetPos = [0.0] * n_joints maxVelocities = [radians(1)] * n_joints maxForces = [0.0] * n_joints for i in range(n_joints): jointIndices[i] = i maxForces[i] = p.getJointInfo(teoId,i)[10] targetPos[i] = 0 rightLegAngles= ik.rl_com_from_foot(com_0 , orientation) leftLegAngles = ik.ll_com_from_foot(com_0 , orientation) ''' print("com =",com_0) print("com_f =",com_0+np.array([0,l16,-l12])) print() print("Hst0_r =",[0,-l16,l9+l10+l11+l12]) print("rightLegP(com_0) =",rightLegP(com_0)) print("rightLegAngles =",[degrees(q) for q in rightLegAngles]) print() print("Hst0_l =",[0,l16,l9+l10+l11+l12]) print("leftLegP(com_0) =",leftLegP(com_0)) print("leftLegAngles =",[degrees(q) for q in leftLegAngles]) ''' for i, index in enumerate(rightLegIndices): if (not np.isnan(rightLegAngles[i])) and (rightLegAngles[i] > p.getJointInfo(teoId, index)[8] and rightLegAngles[i] < p.getJointInfo(teoId, index)[9]): targetPos[index]=rightLegAngles[i] for i, index in enumerate(leftLegIndices): if (not np.isnan(leftLegAngles[i])) and ( leftLegAngles[i] > p.getJointInfo(teoId, index)[8] and leftLegAngles[i] < p.getJointInfo(teoId, index)[9]): targetPos[index]=leftLegAngles[i] for jointIndex in range(n_joints): p.resetJointState(teoId, jointIndex, targetPos[jointIndex]) mode = p.POSITION_CONTROL p.setJointMotorControlArray(teoId, jointIndices, controlMode=mode, forces=maxForces, targetPositions = targetPos ) return teoId
return log #clid = p.connect(p.SHARED_MEMORY) p.connect(p.GUI) p.loadURDF("plane.urdf",[0,0,-0.3]) p.loadURDF("kuka_iiwa/model.urdf",[0,0,0]) p.loadURDF("cube.urdf",[2,2,5]) p.loadURDF("cube.urdf",[-2,-2,5]) p.loadURDF("cube.urdf",[2,-2,5]) log = readLogFile("LOG0001.txt") recordNum = len(log) itemNum = len(log[0]) print('record num:'), print(recordNum) print('item num:'), print(itemNum) for record in log: Id = record[2] pos = [record[3],record[4],record[5]] orn = [record[6],record[7],record[8],record[9]] p.resetBasePositionAndOrientation(Id,pos,orn) numJoints = p.getNumJoints(Id) for i in range (numJoints): jointInfo = p.getJointInfo(Id,i) qIndex = jointInfo[3] if qIndex > -1: p.resetJointState(Id,i,record[qIndex-7+17]) sleep(0.0005)