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 robot_specific_reset(self): HumanoidFlagrun.robot_specific_reset(self) self.frame = 0 if (self.aggressive_cube): p.resetBasePositionAndOrientation(self.aggressive_cube.bodies[0],[-1.5,0,0.05],[0,0,0,1]) else: self.aggressive_cube = get_cube(-1.5,0,0.05) self.on_ground_frame_counter = 0 self.crawl_start_potential = None self.crawl_ignored_potential = 0.0 self.initial_z = 0.8
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 flag_reposition(self): self.walk_target_x = self.np_random.uniform(low=-self.scene.stadium_halflen, high=+self.scene.stadium_halflen) self.walk_target_y = self.np_random.uniform(low=-self.scene.stadium_halfwidth, high=+self.scene.stadium_halfwidth) more_compact = 0.5 # set to 1.0 whole football field self.walk_target_x *= more_compact self.walk_target_y *= more_compact if (self.flag): #for b in self.flag.bodies: # print("remove body uid",b) # p.removeBody(b) p.resetBasePositionAndOrientation(self.flag.bodies[0],[self.walk_target_x, self.walk_target_y, 0.7],[0,0,0,1]) else: self.flag = get_sphere(self.walk_target_x, self.walk_target_y, 0.7) self.flag_timeout = 600/self.scene.frame_skip #match Roboschool
def testJacobian(self): import pybullet as p clid = p.connect(p.SHARED_MEMORY) if (clid < 0): p.connect(p.DIRECT) time_step = 0.001 gravity_constant = -9.81 urdfs = [ "TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf", "kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf" ] for urdf in urdfs: p.resetSimulation() p.setTimeStep(time_step) p.setGravity(0.0, 0.0, gravity_constant) robotId = p.loadURDF(urdf, useFixedBase=True) p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1]) numJoints = p.getNumJoints(robotId) endEffectorIndex = numJoints - 1 # Set a joint target for the position control and step the sim. self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)]) p.stepSimulation() # Get the joint and link state directly from Bullet. mpos, mvel, mtorq = self.getMotorJointStates(robotId) result = p.getLinkState(robotId, endEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1) link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result # Get the Jacobians for the CoM of the end-effector link. # Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn. # The localPosition is always defined in terms of the link frame coordinates. zero_vec = [0.0] * len(mpos) jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec, zero_vec) assert (allclose(dot(jac_t, mvel), link_vt)) assert (allclose(dot(jac_r, mvel), link_vr)) p.disconnect()
def reset(self): p.resetBasePositionAndOrientation(self.pybullet_id, self.initial_position, (0., 0., 0., 1.))
import time import glob import common.gym_interface as gym_interface import pybullet as p import os import pybullet_data import shutil import re p.connect(p.GUI) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) files = glob.glob("../input_data/bodies/5*.xml") files.sort() print(files) p.resetSimulation() filename = os.path.join(pybullet_data.getDataPath(), "plane_stadium.sdf") _ = p.loadSDF(filename) robots = [None] * len(files) for i in range(len(files)): (robots[i], ) = p.loadMJCF(files[i]) p.resetBasePositionAndOrientation(robots[i], [i * 2, 0, 1], [0, 0, 0, 1]) while True: p.stepSimulation() time.sleep(0.01)
def run_sim(inp): goal_pos = np.copy(inp) angle = 0. if not p.isConnected(): if not B_VISUALIZE: p.connect(p.DIRECT) else: p.connect(p.GUI) p.resetDebugVisualizerCamera(cameraDistance=2.0, cameraYaw=180 + 45, cameraPitch=-15, cameraTargetPosition=[0.5, 0.5, 0.6]) p.resetSimulation() p.setGravity(0, 0, -9.8) p.setPhysicsEngineParameter(fixedTimeStep=SimConfig.CONTROLLER_DT, numSubSteps=SimConfig.N_SUBSTEP) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) robot = p.loadURDF( cwd + "/robot_model/draco3/draco3_gripper_mesh_updated.urdf", SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT, SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT) p.loadURDF(cwd + "/robot_model/ground/plane.urdf", [0, 0, 0]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config( robot, SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT, SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT, False) if B_VISUALIZE: lh_target_frame = p.loadURDF(cwd + "/robot_model/etc/ball.urdf", [0., 0, 0.], [0, 0, 0, 1]) lh_target_pos = np.array([0., 0., 0.]) lh_target_quat = np.array([0., 0., 0., 1.]) lh_waypoint_frame = p.loadURDF(cwd + "/robot_model/etc/ball.urdf", [0., 0, 0.], [0, 0, 0, 1]) lh_waypoint_pos = np.array([0., 0., 0.]) lh_waypoint_quat = np.array([0., 0., 0., 1.]) # Add Gear constraint c = p.createConstraint(robot, link_id['l_knee_fe_lp'], robot, link_id['l_knee_fe_ld'], jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(c, gearRatio=-1, maxForce=500, erp=10) c = p.createConstraint(robot, link_id['r_knee_fe_lp'], robot, link_id['r_knee_fe_ld'], jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(c, gearRatio=-1, maxForce=500, erp=10) # Initial Config set_initial_config(robot, joint_id) # Link Damping pybullet_util.set_link_damping(robot, link_id.values(), 0., 0.) # Joint Friction pybullet_util.set_joint_friction(robot, joint_id, 0.) gripper_attached_joint_id = OrderedDict() gripper_attached_joint_id["l_wrist_pitch"] = joint_id["l_wrist_pitch"] gripper_attached_joint_id["r_wrist_pitch"] = joint_id["r_wrist_pitch"] pybullet_util.set_joint_friction(robot, gripper_attached_joint_id, 0.1) # Construct Interface interface = DracoManipulationInterface() # Run Sim t = 0 dt = SimConfig.CONTROLLER_DT count = 0 nominal_sensor_data = pybullet_util.get_sensor_data( robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom) if B_VISUALIZE: pybullet_util.draw_link_frame(robot, link_id['l_hand_contact'], text="lh") pybullet_util.draw_link_frame(lh_target_frame, -1, text="lh_target") pybullet_util.draw_link_frame(lh_waypoint_frame, -1) gripper_command = dict() for gripper_joint in GRIPPER_JOINTS: gripper_command[gripper_joint] = nominal_sensor_data['joint_pos'][ gripper_joint] waiting_command = True time_command_recved = 0. while (1): # Get SensorData sensor_data = pybullet_util.get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom) for gripper_joint in GRIPPER_JOINTS: del sensor_data['joint_pos'][gripper_joint] del sensor_data['joint_vel'][gripper_joint] rf_height = pybullet_util.get_link_iso(robot, link_id['r_foot_contact'])[2, 3] lf_height = pybullet_util.get_link_iso(robot, link_id['l_foot_contact'])[2, 3] sensor_data['b_rf_contact'] = True if rf_height <= 0.01 else False sensor_data['b_lf_contact'] = True if lf_height <= 0.01 else False if t >= WalkingConfig.INIT_STAND_DUR + 0.1 and waiting_command: global_goal_pos = np.copy(goal_pos) global_goal_pos[0] += sensor_data['base_com_pos'][0] global_goal_pos[1] += sensor_data['base_com_pos'][1] lh_target_pos = np.copy(global_goal_pos) lh_target_rot = np.dot(RIGHTUP_GRIPPER, x_rot(angle)) lh_target_quat = util.rot_to_quat(lh_target_rot) lh_target_iso = liegroup.RpToTrans(lh_target_rot, lh_target_pos) lh_waypoint_pos = generate_keypoint(lh_target_iso)[0:3, 3] interface.interrupt_logic.lh_target_pos = lh_target_pos interface.interrupt_logic.lh_waypoint_pos = lh_waypoint_pos interface.interrupt_logic.lh_target_quat = lh_target_quat interface.interrupt_logic.b_interrupt_button_one = True waiting_command = False time_command_recved = t command = interface.get_command(copy.deepcopy(sensor_data)) if B_VISUALIZE: p.resetBasePositionAndOrientation(lh_target_frame, lh_target_pos, lh_target_quat) p.resetBasePositionAndOrientation(lh_waypoint_frame, lh_waypoint_pos, lh_target_quat) # Exclude Knee Proximal Joints Command del command['joint_pos']['l_knee_fe_jp'] del command['joint_pos']['r_knee_fe_jp'] del command['joint_vel']['l_knee_fe_jp'] del command['joint_vel']['r_knee_fe_jp'] del command['joint_trq']['l_knee_fe_jp'] del command['joint_trq']['r_knee_fe_jp'] # Apply Command pybullet_util.set_motor_trq(robot, joint_id, command['joint_trq']) pybullet_util.set_motor_pos(robot, joint_id, gripper_command) p.stepSimulation() t += dt count += 1 ## Safety On the run safe_list = is_safe(robot, link_id, sensor_data) if all(safe_list): pass else: safe_list.append(False) del interface break ## Safety at the end if t >= time_command_recved + ManipulationConfig.T_REACHING_DURATION + 0.2: if is_tracking_error_safe(global_goal_pos, angle, robot, link_id): safe_list.append(True) else: safe_list.append(False) del interface break return safe_list
import pybullet as p import time import math from datetime import datetime #clid = p.connect(p.SHARED_MEMORY) p.connect(p.GUI) p.loadURDF("plane.urdf", [0, 0, -0.3]) kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0]) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1]) kukaEndEffectorIndex = 6 numJoints = p.getNumJoints(kukaId) 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.
import pybullet as p import time import math from datetime import datetime clid = p.connect(p.SHARED_MEMORY) if (clid < 0): p.connect(p.GUI) p.loadURDF("plane.urdf", [0, 0, -1.3]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) sawyerId = p.loadURDF("pole.urdf", [0, 0, 0]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1]) sawyerEndEffectorIndex = 3 numJoints = p.getNumJoints(sawyerId) #joint damping coefficents jd = [0.1, 0.1, 0.1, 0.1] p.setGravity(0, 0, 0) t = 0. prevPose = [0, 0, 0] prevPose1 = [0, 0, 0] hasPrevPose = 0 ikSolver = 0 useRealTimeSimulation = 0 p.setRealTimeSimulation(useRealTimeSimulation) #trailDuration is duration (in seconds) after debug lines will be removed automatically #use 0 for no-removal trailDuration = 1
#time.sleep(1.0) #raw_input("Press Enter to continue...") for i in range(20000): for j in range(3): p.resetJointStateMultiDof(human_adult_ID, j, p.getQuaternionFromEuler([0, 0, 0])) p.resetJointStateMultiDof( human_adult_ID, 3, p.getQuaternionFromEuler([i / 100.0, i / 150.0, i / 350.0])) #for j in range(p.getNumJoints(human_adult_ID)): # p.resetJointStateMultiDof(human_adult_ID, # j, # targetValue=[i/10.0,i/8.0,i/10.0], # targetVelocity=[0.01,0.01,0.01]) p.resetBasePositionAndOrientation(human_adult_ID, [0.1, 0.1, 0.1], p.getQuaternionFromEuler([0, 0, 0])) p.stepSimulation() time.sleep(1. / 240.) #p.getNumJoints(humanoid) #p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel) #for i in range(2000): # pos, ori = p.getBasePositionAndOrientation(human_adult_ID) # p.applyExternalForce(human_adult_ID, -1, # [50*math.cos(i/100.0*math.pi), 50*math.sin(i/88.0*math.pi), 50*math.sin(i/130.0*math.pi)], # pos, p.WORLD_FRAME) # p.stepSimulation() # time.sleep(1./240.) p.disconnect()
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) p.loadURDF("plane.urdf") objects = p.loadMJCF("mjcf/humanoid_symmetric.xml") ob = objects[0] 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
def set_position_orientation(self, pos, orn): p.resetBasePositionAndOrientation(self.body_id, pos, orn)
def reset(self): self.setup_timing() self.task_success = 0 self.contact_points_on_arm = {} self.human, self.bed, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world( furniture_type='bed', static_human_base=False, human_impairment='random', print_joints=False, gender='random') self.robot_lower_limits = self.robot_lower_limits[ self.robot_left_arm_joint_indices] self.robot_upper_limits = self.robot_upper_limits[ self.robot_left_arm_joint_indices] self.reset_robot_joints() friction = 5 p.changeDynamics(self.bed, -1, lateralFriction=friction, spinningFriction=friction, rollingFriction=friction, physicsClientId=self.id) # Setup human in the air and let them settle into a resting pose on the bed joints_positions = [(3, np.deg2rad(30))] controllable_joints = [] self.world_creation.setup_human_joints(self.human, joints_positions, controllable_joints, use_static_joints=False, human_reactive_force=None) p.resetBasePositionAndOrientation(self.human, [-0.15, 0.2, 0.95], p.getQuaternionFromEuler( [-np.pi / 2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id) p.setGravity(0, 0, -1, physicsClientId=self.id) # Add small variation in human joint positions for j in range(p.getNumJoints(self.human, physicsClientId=self.id)): if p.getJointInfo(self.human, j, physicsClientId=self.id)[2] != p.JOINT_FIXED: p.resetJointState(self.human, jointIndex=j, targetValue=self.np_random.uniform( -0.1, 0.1), targetVelocity=0, physicsClientId=self.id) # Let the person settle on the bed for _ in range(100): p.stepSimulation(physicsClientId=self.id) # Lock human joints and set velocities to 0 joints_positions = [] self.human_controllable_joint_indices = [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9 ] if self.human_control else [] self.world_creation.setup_human_joints( self.human, joints_positions, self.human_controllable_joint_indices, use_static_joints=True, human_reactive_force=None, human_reactive_gain=0.01) self.target_human_joint_positions = [] if self.human_control: human_joint_states = p.getJointStates( self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id) self.target_human_joint_positions = np.array( [x[0] for x in human_joint_states]) self.human_lower_limits = self.human_lower_limits[ self.human_controllable_joint_indices] self.human_upper_limits = self.human_upper_limits[ self.human_controllable_joint_indices] p.changeDynamics(self.human, -1, mass=0, physicsClientId=self.id) p.resetBaseVelocity(self.human, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], physicsClientId=self.id) p.setGravity(0, 0, 0, physicsClientId=self.id) p.setGravity(0, 0, -1, body=self.human, physicsClientId=self.id) # Find the base position and joint positions for a static person in bed # print(p.getBasePositionAndOrientation(self.human, physicsClientId=self.id)) # joint_states = p.getJointStates(self.human, jointIndices=list(range(p.getNumJoints(self.human, physicsClientId=self.id))), physicsClientId=self.id) # joint_positions = np.array([x[0] for x in joint_states]) # joint_string = '[' # for i, jp in enumerate(joint_positions): # joint_string += '(%d, %.4f), ' % (i, jp) # print(joint_string + ']') # exit() shoulder_pos, shoulder_orient = p.getLinkState( self.human, 5, computeForwardKinematics=True, physicsClientId=self.id)[:2] elbow_pos, elbow_orient = p.getLinkState(self.human, 7, computeForwardKinematics=True, physicsClientId=self.id)[:2] wrist_pos, wrist_orient = p.getLinkState(self.human, 9, computeForwardKinematics=True, physicsClientId=self.id)[:2] target_pos = np.array([-0.6, 0.2, 1]) + self.np_random.uniform( -0.05, 0.05, size=3) if self.robot_type == 'pr2': target_orient = np.array( p.getQuaternionFromEuler(np.array([0, 0, 0]), physicsClientId=self.id)) self.position_robot_toc( self.robot, 76, [(target_pos, target_orient)], [(shoulder_pos, None), (elbow_pos, None), (wrist_pos, None)], self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=range(29, 29 + 7), pos_offset=np.array([-0.1, 0, 0]), max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) self.world_creation.set_gripper_open_position(self.robot, position=0.2, left=True, set_instantly=True) self.tool = self.world_creation.init_tool( self.robot, mesh_scale=[1] * 3, pos_offset=[0, 0, 0], orient_offset=p.getQuaternionFromEuler( [0, 0, 0], physicsClientId=self.id), maximal=False) elif self.robot_type == 'jaco': target_orient = p.getQuaternionFromEuler(np.array( [0, np.pi / 2.0, 0]), physicsClientId=self.id) base_position, base_orientation, _ = self.position_robot_toc( self.robot, 8, [(target_pos, target_orient)], [(shoulder_pos, None), (elbow_pos, None), (wrist_pos, None)], self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=[0, 1, 2, 3, 4, 5, 6], pos_offset=np.array([-0.05, 1.05, 0.6]), max_ik_iterations=200, step_sim=True, random_position=0.1, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) self.world_creation.set_gripper_open_position(self.robot, position=1.1, left=True, set_instantly=True) self.tool = self.world_creation.init_tool( self.robot, mesh_scale=[1] * 3, pos_offset=[-0.01, 0, 0.03], orient_offset=p.getQuaternionFromEuler( [0, -np.pi / 2.0, 0], physicsClientId=self.id), maximal=False) # Load a nightstand in the environment for the jaco arm self.nightstand_scale = 0.275 visual_filename = os.path.join(self.world_creation.directory, 'nightstand', 'nightstand.obj') collision_filename = os.path.join(self.world_creation.directory, 'nightstand', 'nightstand.obj') nightstand_visual = p.createVisualShape( shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=[self.nightstand_scale] * 3, rgbaColor=[0.5, 0.5, 0.5, 1.0], physicsClientId=self.id) nightstand_collision = p.createCollisionShape( shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=[self.nightstand_scale] * 3, physicsClientId=self.id) nightstand_pos = np.array([-0.9, 0.7, 0]) + base_position nightstand_orient = p.getQuaternionFromEuler( np.array([np.pi / 2.0, 0, 0]), physicsClientId=self.id) self.nightstand = p.createMultiBody( baseMass=0, baseCollisionShapeIndex=nightstand_collision, baseVisualShapeIndex=nightstand_visual, basePosition=nightstand_pos, baseOrientation=nightstand_orient, baseInertialFramePosition=[0, 0, 0], useMaximalCoordinates=False, physicsClientId=self.id) else: target_orient = p.getQuaternionFromEuler(np.array( [0, np.pi / 2.0, 0]), physicsClientId=self.id) if self.robot_type == 'baxter': self.position_robot_toc( self.robot, 48, [(target_pos, target_orient)], [(shoulder_pos, None), (elbow_pos, None), (wrist_pos, None)], self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=range(10, 17), pos_offset=np.array([-0.2, 0, 0.975]), max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) else: self.position_robot_toc( self.robot, 19, [(target_pos, target_orient)], [(shoulder_pos, None), (elbow_pos, None), (wrist_pos, None)], self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=[0, 2, 3, 4, 5, 6, 7], pos_offset=np.array([-0.2, 0, 0.975]), max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) self.world_creation.set_gripper_open_position(self.robot, position=0.0125, left=True, set_instantly=True) self.tool = self.world_creation.init_tool( self.robot, mesh_scale=[0.001] * 3, pos_offset=[0, 0.1175, 0], orient_offset=p.getQuaternionFromEuler( [np.pi / 2.0, 0, np.pi / 2.0], physicsClientId=self.id), maximal=False) self.generate_targets() # Enable rendering p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id) return self._get_obs([0], [0, 0])
def get_total_force(self): total_force = 0 tool_force = 0 tool_force_on_human = 0 total_force_on_human = 0 new_contact_points = 0 for c in p.getContactPoints(bodyA=self.tool, physicsClientId=self.id): total_force += c[9] tool_force += c[9] for c in p.getContactPoints(bodyA=self.robot, physicsClientId=self.id): bodyB = c[2] if bodyB != self.tool: total_force += c[9] for c in p.getContactPoints(bodyA=self.robot, bodyB=self.human, physicsClientId=self.id): total_force_on_human += c[9] for c in p.getContactPoints(bodyA=self.tool, bodyB=self.human, physicsClientId=self.id): linkA = c[3] linkB = c[4] contact_position = np.array(c[6]) total_force_on_human += c[9] if linkA in [1]: tool_force_on_human += c[9] # Contact with human upperarm, forearm, hand if linkB < 0 or linkB > p.getNumJoints( self.human, physicsClientId=self.id): continue indices_to_delete = [] for i, (target_pos_world, target) in enumerate( zip(self.targets_pos_upperarm_world, self.targets_upperarm)): if np.linalg.norm(contact_position - target_pos_world) < 0.025: # The robot made contact with a point on the person's arm new_contact_points += 1 self.task_success += 1 p.resetBasePositionAndOrientation( target, [1000, 1000, 1000], [0, 0, 0, 1], physicsClientId=self.id) indices_to_delete.append(i) self.targets_pos_on_upperarm = [ t for i, t in enumerate(self.targets_pos_on_upperarm) if i not in indices_to_delete ] self.targets_upperarm = [ t for i, t in enumerate(self.targets_upperarm) if i not in indices_to_delete ] self.targets_pos_upperarm_world = [ t for i, t in enumerate(self.targets_pos_upperarm_world) if i not in indices_to_delete ] indices_to_delete = [] for i, (target_pos_world, target) in enumerate( zip(self.targets_pos_forearm_world, self.targets_forearm)): if np.linalg.norm(contact_position - target_pos_world) < 0.025: # The robot made contact with a point on the person's arm new_contact_points += 1 self.task_success += 1 p.resetBasePositionAndOrientation( target, [1000, 1000, 1000], [0, 0, 0, 1], physicsClientId=self.id) indices_to_delete.append(i) self.targets_pos_on_forearm = [ t for i, t in enumerate(self.targets_pos_on_forearm) if i not in indices_to_delete ] self.targets_forearm = [ t for i, t in enumerate(self.targets_forearm) if i not in indices_to_delete ] self.targets_pos_forearm_world = [ t for i, t in enumerate(self.targets_pos_forearm_world) if i not in indices_to_delete ] return total_force, tool_force, tool_force_on_human, total_force_on_human, new_contact_points
humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1]) humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1]) humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1]) humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1]) startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447, 1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153, 0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495, 0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348] startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-0.1050802848,0,0.935706195,0.08277326387,0.3002461862,0,0,0,0,0,1.114409628,0.3618553952, -0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0, -4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973] p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1]) p.resetBasePositionAndOrientation(humanoid2, startLocations[1],[0,0,0,1]) p.resetBasePositionAndOrientation(humanoid3, startLocations[2],[0,0,0,1]) p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1]) index0=7 for j in range (p.getNumJoints(humanoid)): ji = p.getJointInfo(humanoid,j) targetPosition=[0] jointType = ji[2] if (jointType == p.JOINT_SPHERICAL): targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]] targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]] index0+=4
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) ] objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf") kuka_gripper = objects[0] print("kuka gripper=") print(kuka_gripper) p.resetBasePositionAndOrientation(kuka_gripper, [0.923103, -0.200000, 1.250036], [-0.000000, 0.964531, -0.000002, -0.263970]) jointPositions = [ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ] for jointIndex in range(p.getNumJoints(kuka_gripper)): p.resetJointState(kuka_gripper, jointIndex, jointPositions[jointIndex]) p.setJointMotorControl2(kuka_gripper, jointIndex, p.POSITION_CONTROL, jointPositions[jointIndex], 0) kuka_cid = p.createConstraint(kuka, 6, kuka_gripper, 0, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0.05], [0, 0, 0]) objects = [ p.loadURDF("table/table.urdf", 1.000000, -0.200000, 0.000000, 0.000000, 0.000000, 0.707107, 0.707107) ]
import pybullet as p import pybullet_data import time p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadSDF("stadium.sdf") p.setGravity(0, 0, -10) objects = p.loadMJCF("mjcf/sphere.xml") sphere = objects[0] p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1]) p.changeDynamics(sphere, -1, linearDamping=0.9) p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1]) forward = 0 turn = 0 forwardVec = [2, 0, 0] cameraDistance = 1 cameraYaw = 35 cameraPitch = -35 while (1): spherePos, orn = p.getBasePositionAndOrientation(sphere) cameraTargetPosition = spherePos p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition) camInfo = p.getDebugVisualizerCamera() camForward = camInfo[5] keys = p.getKeyboardEvents()
import pybullet as p import time import math from datetime import datetime import pybullet_data clid = p.connect(p.SHARED_MEMORY) if (clid < 0): p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf", [0, 0, -.98]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) sawyerId = p.loadURDF("rudis_magic_sawyer.urdf", [0, 0, 0]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1]) ### MY CODE ### END OF MY CODE #bad, get it from name! sawyerEndEffectorIndex = 18 sawyerEndEffectorIndex = 16 numJoints = p.getNumJoints(sawyerId) #joint damping coefficents jd = [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] p.setGravity(0, 0, 0) t = 0. prevPose = [0, 0, 0]
def update_feet_tasks(self, k_loop, gait, looping, interface, ftps_Ids_deb): """Update the 3D desired position for feet in swing phase by using a 5-th order polynomial that lead them to the desired position on the ground (computed by the footstep planner) Args: k_loop (int): number of time steps since the start of the current gait cycle pair (int): the current pair of feet in swing phase, for a walking trot gait looping (int): total number of time steps in one gait cycle interface (Interface object): interface between the simulator and the MPC/InvDyn ftps_Ids_deb (list): IDs of debug spheres in PyBullet """ # Indexes of feet in swing phase feet = np.where(gait[0, 1:] == 0)[0] if len(feet) == 0: # If no foot in swing phase return 0 t0s = [] for i in feet: # For each foot in swing phase get remaining duration of the swing phase # Index of the line containing the next stance phase index = next((idx for idx, val in np.ndenumerate(gait[:, 1+i]) if (((val==1)))), [-1])[0] remaining_iterations = np.cumsum(gait[:index, 0])[-1] * self.k_mpc - (k_loop % self.k_mpc) # Compute total duration of current swing phase i_iter = 1 self.t_swing[i] = gait[0, 0] while gait[i_iter, 1+i] == 0: self.t_swing[i] += gait[i_iter, 0] i_iter += 1 i_iter = -1 while gait[i_iter, 1+i] == 0: self.t_swing[i] += gait[i_iter, 0] i_iter -= 1 self.t_swing[i] *= self.dt * self.k_mpc t0s.append(np.round(self.t_swing[i] - remaining_iterations * self.dt, decimals=3)) # self.footsteps contains the target (x, y) positions for both feet in swing phase for i in range(len(feet)): i_foot = feet[i] # Get desired 3D position, velocity and acceleration if t0s[i] == 0.000: [x0, dx0, ddx0, y0, dy0, ddy0, z0, dz0, ddz0, gx1, gy1] = (self.ftgs[i_foot]).get_next_foot( interface.o_feet[0, i_foot], interface.ov_feet[0, i_foot], interface.oa_feet[0, i_foot], interface.o_feet[1, i_foot], interface.ov_feet[1, i_foot], interface.oa_feet[1, i_foot], self.footsteps[0, i_foot], self.footsteps[1, i_foot], t0s[i], self.t_swing[i_foot], self.dt) self.mgoals[:, i_foot] = np.array([x0, dx0, ddx0, y0, dy0, ddy0]) else: [x0, dx0, ddx0, y0, dy0, ddy0, z0, dz0, ddz0, gx1, gy1] = (self.ftgs[i_foot]).get_next_foot( self.mgoals[0, i_foot], self.mgoals[1, i_foot], self.mgoals[2, i_foot], self.mgoals[3, i_foot], self.mgoals[4, i_foot], self.mgoals[5, i_foot], self.footsteps[0, i_foot], self.footsteps[1, i_foot], t0s[i], self.t_swing[i_foot], self.dt) self.mgoals[:, i_foot] = np.array([x0, dx0, ddx0, y0, dy0, ddy0]) # Take into account vertical offset of Pybullet z0 += interface.mean_feet_z # Store desired position, velocity and acceleration for later call to this function self.goals[:, i_foot] = np.array([x0, y0, z0]) self.vgoals[:, i_foot] = np.array([dx0, dy0, dz0]) self.agoals[:, i_foot] = np.array([ddx0, ddy0, ddz0]) # Update desired pos, vel, acc self.sampleFeet[i_foot].pos(np.matrix([x0, y0, z0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]).T) self.sampleFeet[i_foot].vel(np.matrix([dx0, dy0, dz0, 0.0, 0.0, 0.0]).T) self.sampleFeet[i_foot].acc(np.matrix([ddx0, ddy0, ddz0, 0.0, 0.0, 0.0]).T) # Set reference self.feetTask[i_foot].setReference(self.sampleFeet[i_foot]) # Update footgoal for display purpose self.feetGoal[i_foot].translation = np.matrix([x0, y0, z0]).T # Display the goal position of the feet as green sphere in PyBullet pyb.resetBasePositionAndOrientation(ftps_Ids_deb[i_foot], posObj=np.array([gx1, gy1, 0.0]), ornObj=np.array([0.0, 0.0, 0.0, 1.0])) return 0
def set_position(self, pos): _, old_orn = p.getBasePositionAndOrientation(self.body_id) p.resetBasePositionAndOrientation(self.body_id, pos, old_orn)
def runSim(self, net, weights, runType=pbl.DIRECT): physicsClient = pbl.connect(runType) pbl.setAdditionalSearchPath(pybullet_data.getDataPath()) pbl.setGravity(0, 0, -9.8) ObstacleIDs = [] ObstacleLocations = [] ObstacleOrientations = [] ObstacleIDs.append( pbl.loadURDF("/Misc/Enclosure/Wall.urdf", basePosition=[0, -1, 5.5])) ObstacleIDs.append( pbl.loadURDF("/Misc/Enclosure/Wall.urdf", basePosition=[0, 11, 5.5])) ObstacleIDs.append( pbl.loadURDF("/Misc/Enclosure/Wall.urdf", basePosition=[-6, 5, 5.5], baseOrientation=pbl.getQuaternionFromEuler( [0, 0, -np.pi / 2]))) ObstacleIDs.append( pbl.loadURDF("/Misc/Enclosure/Wall.urdf", basePosition=[6, 5, 5.5], baseOrientation=pbl.getQuaternionFromEuler( [0, 0, -np.pi / 2]))) ObstacleIDs.append( pbl.loadURDF("/Misc/Enclosure/Wall.urdf", basePosition=[0, 5, 11.5], baseOrientation=pbl.getQuaternionFromEuler( [-np.pi / 2, 0, 0]))) [(ObstacleLocations.append(pbl.getBasePositionAndOrientation(obs)[0]), ObstacleOrientations.append( pbl.getBasePositionAndOrientation(obs)[1])) for obs in ObstacleIDs] dir = np.random.normal(0, 1, size=(3, )) dir /= np.linalg.norm(dir) goal = np.array([ np.random.uniform(-4, 4), np.random.uniform(0, 8), np.random.uniform(0, 4) ]) start = goal + dir * self.L ObstacleIDs, ObstacleLocations = self.generateObstacles( 5, start, ObstacleIDs, ObstacleLocations, ObstacleLocations) Qimmiq = Drone(ObstacleIDs, net=net, init_pos=start, goal=goal) i = 0 Collided = False while (not Qimmiq.targetReached) and not Collided and Qimmiq.time < 20: for o in range(len(Qimmiq.obstacles[:-1])): if self.hasCollided(Qimmiq, Qimmiq.obstacles[o]): Collided = True print('Collision!') break [ pbl.resetBasePositionAndOrientation(ObstacleIDs[i], ObstacleLocations[i], ObstacleOrientations[i]) for i in range(len(ObstacleOrientations)) ] if i % 240 == 0: velocities = [ np.random.normal(size=(3, )) for k in range(len(ObstacleIDs[:-1])) ] [ pbl.resetBaseVelocity(ObstacleIDs[j], velocities[j], [0, 0, 0]) for j in range(len(ObstacleIDs[len(ObstacleOrientations):-1])) ] i += 1 Qimmiq.moveDrone(Qimmiq.time_interval, i) pbl.stepSimulation() pbl.disconnect() print("lifeTime: {}".format(Qimmiq.time)) print("Intrusion: {}".format(Qimmiq.Intrusion)) print("ITSE: {}".format(10 * Qimmiq.e)) # return Qimmiq.time print("Fitness: {0}".format( -1 * (weights[0] * Qimmiq.e + weights[1] * Qimmiq.Intrusion))) return -1 * (weights[0] * Qimmiq.e + weights[1] * Qimmiq.Intrusion)
p.connect(p.DIRECT) time_step = 0.001 gravity_constant = -9.81 p.resetSimulation() p.setTimeStep(time_step) p.setGravity(0.0, 0.0, gravity_constant) p.loadURDF("plane.urdf",[0,0,-0.3]) kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True) #kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0]) #kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0]) #kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0]) #kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0]) p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1]) numJoints = p.getNumJoints(kukaId) kukaEndEffectorIndex = numJoints - 1 # Set a joint target for the position control and step the sim. setJointPosition(kukaId, [0.1] * numJoints) p.stepSimulation() # Get the joint and link state directly from Bullet. pos, vel, torq = getJointStates(kukaId) mpos, mvel, mtorq = getMotorJointStates(kukaId) result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1) link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result # Get the Jacobians for the CoM of the end-effector link. # Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
def reset_orientation(self, orientation): p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation)
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_pose(self, position, orientation): p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, orientation)
print(d[0]) while True: with open("../input_data/position4photos/positions.yml", "r") as f: data = yaml.load(f, Loader=yaml.SafeLoader) data = data[gym_interface.template(body)] joint_positions = p.getJointStates(robot, list(range(p.getNumJoints(robot)))) joint_positions = np.zeros(p.getNumJoints(robot)) if "position" in data: robot_position = [0, 0, 0] for key in data["position"]: robot_position[key] = data["position"][key] p.resetBasePositionAndOrientation(robot, robot_position, [0, 0, 0, 1]) if "joints" in data: for key in data["joints"]: joint_positions[key] = data["joints"][key] if "joints_name" in data: for key in data["joints_name"]: joint_positions[joint_names[key]] = data["joints_name"][key] for joint_id in range(p.getNumJoints(robot)): if joint_positions[joint_id] != 0: p.resetJointState(robot, joint_id, targetValue=joint_positions[joint_id])
def set_config(robot, joint_id, link_id, base_pos, base_quat, joint_pos): p.resetBasePositionAndOrientation(robot, base_pos, base_quat) for k, v in joint_pos.items(): p.resetJointState(robot, joint_id[k], v, 0.)
os.sys.path.insert(0, currentdir) import pybullet_data from pkg_resources import parse_version kuka_handId = 3 xpos = 0.43 ypos = 0.635 zpos = 0.909 ang = 3.14 * 0.5 orn = p.getQuaternionFromEuler([0, 0, ang]) p.connect(p.GUI) finger = p.loadSDF("./model.sdf") fingerID = finger[0] print("fingerID:::", fingerID) p.resetBasePositionAndOrientation(fingerID, [0.00000, 0.200000, 0.65000], [0.000000, 0.000000, 0.000000, 1.000000]) table = p.loadURDF( os.path.join(pybullet_data.getDataPath(), "table/table.urdf"), 0.5000000, 0.60000, 0.0000, 0.000000, 0.000000, 0.0, 1.0) texUid = p.loadTexture("./cube_new/aaa.png") cube_objects = p.loadSDF("./cube_new/model.sdf") cubeId = cube_objects[0] p.changeVisualShape(cube_objects[0], -1, rgbaColor=[1, 1, 1, 1]) p.changeVisualShape(cube_objects[0], -1, textureUniqueId=texUid) p.resetBasePositionAndOrientation(cube_objects[0], (0.5000000, 0.60000, 0.6700), (0.717, 0.0, 0.0, 0.717)) #blockUid = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"block.urdf"), xpos,ypos,zpos,orn[0],orn[1],orn[2],orn[3]) blockUid = 2 p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), [0, 0, 0])
def resetPose(self, pos, rot): pb.resetBasePositionAndOrientation(self.object_id, pos, rot)
p.createMultiBody(0, baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeIds, baseOrientation=orn) filename = os.path.expanduser("~/data/shuri-castle-shurijo-naha-okinawa-wip/source/Shujiro_Castle/Shujiro_Castle.obj") collisionShapeId = p.createCollisionShape(p.GEOM_MESH, fileName=filename, flags=p.GEOM_FORCE_CONCAVE_TRIMESH) visualShapeIds = p.createVisualShape(p.GEOM_MESH, fileName=filename) orn = p.getQuaternionFromEuler([0, 0, 0]) p.createMultiBody(0, baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeIds, baseOrientation=orn) ''' print(p.getBodyInfo(collisionShapeId)) sphere = p.loadURDF("sphere_small.urdf") #objects = p.loadMJCF("mjcf/sphere.xml") #sphere = objects[0] p.resetBasePositionAndOrientation(sphere, [0, 0, startHeight], [0, 0, 0, 1]) p.changeDynamics(sphere, -1, linearDamping=linearDamping) p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1]) p.setGravity(0, 0, -10) forward = 0 turn = 0 forwardVec = [2, 0, 0] cameraDistance = 1 cameraYaw = 35 cameraPitch = -35 joystick = None pygame.init() if pygame.joystick.get_count():
0.170571, 0.9927545808, -0.02090099117, 0.08882396249, -0.07817796699, -0.391532, 0.9828788495, 0.1013909845, -0.05515999155, 0.143618978, 0.9659421276, 0.1884590249, -0.1422460188, 0.105854014, 0.581348 ] startVel = [ 1.235314324, -0.008525509087, 0.1515293946, -1.161516553, 0.1866449799, -0.1050802848, 0, 0.935706195, 0.08277326387, 0.3002461862, 0, 0, 0, 0, 0, 1.114409628, 0.3618553952, -0.4505575061, 0, -1.725374735, -0.5052852598, -0.8555179722, -0.2221173515, 0, -0.1837617357, 0.00171895706, 0.03912837591, 0, 0.147945294, 1.837653345, 0.1534535548, 1.491385941, 0, -4.632454387, -0.9111172777, -1.300648184, -1.345694622, 0, -1.084238535, 0.1313680236, -0.7236998534, 0, -0.5278312973 ] p.resetBasePositionAndOrientation(humanoid, startLocations[0], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(humanoid2, startLocations[1], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(humanoid4, startLocations[3], [0, 0, 0, 1]) index0 = 7 for j in range(p.getNumJoints(humanoid)): ji = p.getJointInfo(humanoid, j) targetPosition = [0] jointType = ji[2] if (jointType == p.JOINT_SPHERICAL): targetPosition = [ startPose[index0 + 1], startPose[index0 + 2], startPose[index0 + 3], startPose[index0 + 0] ] targetVel = [
import pybullet as p import pybullet_data import time import pybullet_data p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadSDF("stadium.sdf") p.setGravity(0, 0, -10) objects = p.loadMJCF("mjcf/sphere.xml") sphere = objects[0] p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1]) p.changeDynamics(sphere, -1, linearDamping=0.9) p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1]) forward = 0 turn = 0 forwardVec = [2, 0, 0] cameraDistance = 1 cameraYaw = 35 cameraPitch = -35 while (1): spherePos, orn = p.getBasePositionAndOrientation(sphere) cameraTargetPosition = spherePos p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition) camInfo = p.getDebugVisualizerCamera()
] 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) ] objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf") kuka_gripper = objects[0] print("kuka gripper=") print(kuka_gripper) p.resetBasePositionAndOrientation(kuka_gripper, [0.923103, -0.200000, 1.250036], [-0.000000, 0.964531, -0.000002, -0.263970]) jointPositions = [ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ] for jointIndex in range(p.getNumJoints(kuka_gripper)): p.resetJointState(kuka_gripper, jointIndex, jointPositions[jointIndex]) p.setJointMotorControl2(kuka_gripper, jointIndex, p.POSITION_CONTROL, jointPositions[jointIndex], 0) kuka_cid = p.createConstraint(kuka, 6, kuka_gripper, 0, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0.05], [0, 0, 0]) objects = [ p.loadURDF("jenga/jenga.urdf", 1.300000, -0.700000, 0.750000, 0.000000,
def _set_fields_of_pose_of(self, pos, orn): """Set pose of body part""" p.resetBasePositionAndOrientation(self.bodies[self.body_index], pos, orn)
def reset(self): self.setup_timing() self.task_success = 0 self.human, self.bed, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world( furniture_type='bed', static_human_base=False, human_impairment='no_tremor', print_joints=False, gender='random') self.robot_both_arm_joint_indices = self.robot_left_arm_joint_indices + self.robot_right_arm_joint_indices self.robot_right_lower_limits = self.robot_lower_limits[ self.robot_right_arm_joint_indices] self.robot_right_upper_limits = self.robot_upper_limits[ self.robot_right_arm_joint_indices] self.robot_left_lower_limits = self.robot_lower_limits[ self.robot_left_arm_joint_indices] self.robot_left_upper_limits = self.robot_upper_limits[ self.robot_left_arm_joint_indices] self.robot_lower_limits = self.robot_lower_limits[ self.robot_both_arm_joint_indices] self.robot_upper_limits = self.robot_upper_limits[ self.robot_both_arm_joint_indices] self.reset_robot_joints() friction = 5 p.changeDynamics(self.bed, -1, lateralFriction=friction, spinningFriction=friction, rollingFriction=friction, physicsClientId=self.id) # Setup human in the air and let them settle into a resting pose on the bed joints_positions = [(3, np.deg2rad(30))] controllable_joints = [] self.world_creation.setup_human_joints(self.human, joints_positions, controllable_joints, use_static_joints=False, human_reactive_force=None) p.resetBasePositionAndOrientation(self.human, [-0.25, 0.2, 0.95], p.getQuaternionFromEuler( [-np.pi / 2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id) p.setGravity(0, 0, -1, physicsClientId=self.id) p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id) # Add small variation in human joint positions for j in range(p.getNumJoints(self.human, physicsClientId=self.id)): if p.getJointInfo(self.human, j, physicsClientId=self.id)[2] != p.JOINT_FIXED: p.resetJointState(self.human, jointIndex=j, targetValue=self.np_random.uniform( -0.1, 0.1), targetVelocity=0, physicsClientId=self.id) # Let the person settle on the bed for _ in range(100): p.stepSimulation(physicsClientId=self.id) friction = 0.3 p.changeDynamics(self.bed, -1, lateralFriction=friction, spinningFriction=friction, rollingFriction=friction, physicsClientId=self.id) # Lock human joints (except arm) and set velocities to 0 joints_positions = [(3, np.deg2rad(60)), (4, np.deg2rad(-60)), (6, 0)] self.human_controllable_joint_indices = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9] self.world_creation.setup_human_joints( self.human, joints_positions, self.human_controllable_joint_indices, use_static_joints=True, human_reactive_force=None) p.changeDynamics(self.human, -1, mass=0, physicsClientId=self.id) p.resetBaseVelocity(self.human, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], physicsClientId=self.id) for i in self.human_controllable_joint_indices: p.resetJointState(self.human, i, targetValue=p.getJointState( self.human, i, physicsClientId=self.id)[0], targetVelocity=0, physicsClientId=self.id) for _ in range(100): p.stepSimulation(physicsClientId=self.id) human_joint_states = p.getJointStates( self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id) self.target_human_joint_positions = np.array( [x[0] for x in human_joint_states]) self.human_lower_limits = self.human_lower_limits[ self.human_controllable_joint_indices] self.human_upper_limits = self.human_upper_limits[ self.human_controllable_joint_indices] shoulder_pos, shoulder_orient = p.getLinkState( self.human, 5, computeForwardKinematics=True, physicsClientId=self.id)[:2] elbow_pos, elbow_orient = p.getLinkState(self.human, 7, computeForwardKinematics=True, physicsClientId=self.id)[:2] wrist_pos, wrist_orient = p.getLinkState(self.human, 9, computeForwardKinematics=True, physicsClientId=self.id)[:2] waist_pos = p.getLinkState(self.human, 24, computeForwardKinematics=True, physicsClientId=self.id)[0] hips_pos = p.getLinkState(self.human, 27, computeForwardKinematics=True, physicsClientId=self.id)[0] if self.robot_type == 'jaco': bed_pos, bed_orient = p.getBasePositionAndOrientation( self.bed, physicsClientId=self.id) p.resetBasePositionAndOrientation( self.robot, np.array(bed_pos) + np.array([-0.7, 0.75, 0.6]), p.getQuaternionFromEuler([0, 0, -np.pi / 2.0], physicsClientId=self.id), physicsClientId=self.id) elif self.robot_type == 'kinova_gen3': bed_pos, bed_orient = p.getBasePositionAndOrientation( self.bed, physicsClientId=self.id) p.resetBasePositionAndOrientation( self.robot, np.array(bed_pos) + np.array([-0.7, 0.75, 0.6]), p.getQuaternionFromEuler([0, 0, -np.pi / 2.0], physicsClientId=self.id), physicsClientId=self.id) target_pos_right = np.array( [-0.9, -0.3, 0.8]) + self.np_random.uniform(-0.05, 0.05, size=3) target_pos_left = np.array([-0.9, 0.7, 0.8]) + self.np_random.uniform( -0.05, 0.05, size=3) if self.robot_type == 'pr2': target_orient = np.array( p.getQuaternionFromEuler(np.array([0, 0, 0]), physicsClientId=self.id)) self.position_robot_toc( self.robot, [54, 77], [[(target_pos_right, target_orient)], [(target_pos_left, target_orient)]], [[(wrist_pos, None), (hips_pos, None)], [(elbow_pos, None), (waist_pos, None)]], [ self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices ], [self.robot_right_lower_limits, self.robot_left_lower_limits], [self.robot_right_upper_limits, self.robot_left_upper_limits], ik_indices=[range(15, 15 + 7), range(29, 29 + 7)], pos_offset=np.array([-0.3, 0.7, 0]), max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) self.world_creation.set_gripper_open_position( self.robot, position=0.15, left=True, set_instantly=True, indices=[81, 82, 83, 84]) self.world_creation.set_gripper_open_position( self.robot, position=0.15, left=False, set_instantly=True, indices=[58, 59, 60, 61]) elif self.robot_type in ['jaco', 'kinova_gen3']: if self.robot_type == 'jaco': target_pos_left = np.array([ -0.9, 0.4, 1 ]) + self.np_random.uniform(-0.05, 0.05, size=3) target_orient = p.getQuaternionFromEuler( np.array([0, np.pi / 2.0, 0]), physicsClientId=self.id) base_position, base_orientation, _ = self.position_robot_toc( self.robot, 8, [(target_pos_left, target_orient)], [(wrist_pos, None), (hips_pos, None), (elbow_pos, None), (waist_pos, None)], self.robot_left_arm_joint_indices, self.robot_left_lower_limits, self.robot_left_upper_limits, ik_indices=[0, 1, 2, 3, 4, 5, 6], pos_offset=np.array([-0.05, 1.15, 0.6]), max_ik_iterations=200, step_sim=True, random_position=0.1, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) self.world_creation.set_gripper_open_position( self.robot, position=1.05, left=False, set_instantly=True, indices=[10, 12, 14]) else: base_position = np.array([-0.05, 0, 0]) target_pos_left = np.array([ -0.9, 0.5, 1 ]) + self.np_random.uniform(-0.05, 0.05, size=3) target_orient = p.getQuaternionFromEuler( np.array([0, np.pi / 2.0, 0]), physicsClientId=self.id) self.util.ik_random_restarts(self.robot, 7, target_pos_left, target_orient, self.world_creation, self.robot_left_arm_joint_indices, self.robot_left_lower_limits, self.robot_left_upper_limits, ik_indices=[0, 1, 2, 3, 4, 5, 6], max_iterations=100, max_ik_random_restarts=40, random_restart_threshold=0.03, step_sim=True) # Load a nightstand in the environment for the jaco arm self.nightstand_scale = 0.275 visual_filename = os.path.join(self.world_creation.directory, 'nightstand', 'nightstand.obj') collision_filename = os.path.join(self.world_creation.directory, 'nightstand', 'nightstand.obj') nightstand_visual = p.createVisualShape( shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=[self.nightstand_scale] * 3, rgbaColor=[0.5, 0.5, 0.5, 1.0], physicsClientId=self.id) nightstand_collision = p.createCollisionShape( shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=[self.nightstand_scale] * 3, physicsClientId=self.id) nightstand_pos = np.array([-0.9, 0.8, 0]) + base_position nightstand_orient = p.getQuaternionFromEuler( np.array([np.pi / 2.0, 0, 0]), physicsClientId=self.id) self.nightstand = p.createMultiBody( baseMass=0, baseCollisionShapeIndex=nightstand_collision, baseVisualShapeIndex=nightstand_visual, basePosition=nightstand_pos, baseOrientation=nightstand_orient, baseInertialFramePosition=[0, 0, 0], useMaximalCoordinates=False, physicsClientId=self.id) else: target_orient = p.getQuaternionFromEuler(np.array( [0, -np.pi / 2.0, np.pi]), physicsClientId=self.id) if self.robot_type == 'baxter': self.position_robot_toc( self.robot, [26, 49], [[(target_pos_right, target_orient)], [(target_pos_left, target_orient)]], [[(wrist_pos, None), (hips_pos, None)], [(elbow_pos, None), (waist_pos, None)]], [ self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices ], [ self.robot_right_lower_limits, self.robot_left_lower_limits ], [ self.robot_right_upper_limits, self.robot_left_upper_limits ], ik_indices=[range(1, 8), range(10, 17)], pos_offset=np.array([-0.3, 0.6, 0.975]), max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) self.world_creation.set_gripper_open_position( self.robot, position=0.01, left=True, set_instantly=True, indices=[50, 52]) self.world_creation.set_gripper_open_position( self.robot, position=0.01, left=False, set_instantly=True, indices=[27, 29]) else: self.position_robot_toc( self.robot, 19, [(target_pos_left, target_orient)], [(wrist_pos, None), (hips_pos, None), (elbow_pos, None), (waist_pos, None)], self.robot_left_arm_joint_indices, self.robot_left_lower_limits, self.robot_left_upper_limits, ik_indices=[0, 2, 3, 4, 5, 6, 7], pos_offset=np.array([-0.3, 0.6, 0.975]), max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions) self.world_creation.set_gripper_open_position( self.robot, position=0.01, left=True, set_instantly=True) p.setGravity(0, 0, -9.81, physicsClientId=self.id) p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id) # Enable rendering p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id) return self._get_obs([0, 0], [0, 0, 0])
def step(self, action): done = False #reward (float): amount of reward achieved by the previous action. The scale varies between environments, but the goal is always to increase your total reward. #done (boolean): whether it's time to reset the environment again. Most (but not all) tasks are divided up into well-defined episodes, and done being True indicates the episode has terminated. (For example, perhaps the pole tipped too far, or you lost your last life.) #observation (object): an environment-specific object representing your observation of the environment. For example, pixel data from a camera, joint angles and joint velocities of a robot, or the board state in a board game. #info (dict): diagnostic information useful for debugging. It can sometimes be useful for learning (for example, it might contain the raw probabilities behind the environment's last state change). However, official evaluations of your agent are not allowed to use this for learning. def convertSensor(finger_index): if finger_index == self.indexId: return random.uniform(-1, 1) #return 0 else: #return random.uniform(-1,1) return 0 def convertAction(action): #converted = (action-30)/10 #converted = (action-16)/10 if action == 6: converted = -1 elif action == 25: converted = 1 #print("action ",action) #print("converted ",converted) return converted aspect = 1 camTargetPos = [0, 0, 0] yaw = 40 pitch = 10.0 roll = 0 upAxisIndex = 2 camDistance = 4 pixelWidth = 320 pixelHeight = 240 nearPlane = 0.0001 farPlane = 0.022 lightDirection = [0, 1, 0] lightColor = [1, 1, 1] #optional fov = 50 #10 or 50 hand_po = pb.getBasePositionAndOrientation(self.agent) #print("action",action) ho = pb.getQuaternionFromEuler([0, 0, 0]) #hand_cid = pb.createConstraint(self.hand,-1,-1,-1,pb.JOINT_FIXED,[0,0,0],(0.1,0,0),hand_po[0],ho,hand_po[1]) if action == 65298 or action == 0: #down #pb.changeConstraint(hand_cid,(hand_po[0][0]+self.move,hand_po[0][1],hand_po[0][2]),hand_po[1], maxForce=50) pb.resetBasePositionAndOrientation( self.agent, (hand_po[0][0] + self.move, hand_po[0][1], hand_po[0][2]), hand_po[1]) elif action == 65297 or action == 1: #up pb.resetBasePositionAndOrientation( self.agent, (hand_po[0][0] - self.move, hand_po[0][1], hand_po[0][2]), hand_po[1]) #pb.changeConstraint(hand_cid,(hand_po[0][0]-self.move,hand_po[0][1],hand_po[0][2]),hand_po[1], maxForce=50) elif action == 65295 or action == 2: #left pb.resetBasePositionAndOrientation( self.agent, (hand_po[0][0], hand_po[0][1] + self.move, hand_po[0][2]), hand_po[1]) #pb.changeConstraint(hand_cid,(hand_po[0][0],hand_po[0][1]+self.move,hand_po[0][2]),hand_po[1], maxForce=50) elif action == 65296 or action == 3: #right pb.resetBasePositionAndOrientation( self.agent, (hand_po[0][0], hand_po[0][1] - self.move, hand_po[0][2]), hand_po[1]) #pb.changeConstraint(hand_cid,(hand_po[0][0],hand_po[0][1]-self.move,hand_po[0][2]),hand_po[1], maxForce=50) elif action == 44 or action == 4: #< pb.resetBasePositionAndOrientation( self.agent, (hand_po[0][0], hand_po[0][1], hand_po[0][2] + self.move), hand_po[1]) #pb.changeConstraint(hand_cid,(hand_po[0][0],hand_po[0][1],hand_po[0][2]+self.move),hand_po[1], maxForce=50) elif action == 46 or action == 5: #> pb.resetBasePositionAndOrientation( self.agent, (hand_po[0][0], hand_po[0][1], hand_po[0][2] - self.move), hand_po[1]) #pb.changeConstraint(hand_cid,(hand_po[0][0],hand_po[0][1],hand_po[0][2]-self.move),hand_po[1], maxForce=50) elif action >= 6 and action <= 7: #elif action >= 6 and action <= 40: if action == 7: action = 25 #bad kludge redo all this code pink = convertSensor(self.pinkId) middle = convertSensor(self.middleId) index = convertAction(action) thumb = convertSensor(self.thumbId) ring = convertSensor(self.ring_id) pb.setJointMotorControl2(self.agent, 7, pb.POSITION_CONTROL, self.pi / 4.) pb.setJointMotorControl2(self.agent, 9, pb.POSITION_CONTROL, thumb + self.pi / 10) pb.setJointMotorControl2(self.agent, 11, pb.POSITION_CONTROL, thumb) pb.setJointMotorControl2(self.agent, 13, pb.POSITION_CONTROL, thumb) # That's index finger parts pb.setJointMotorControl2(self.agent, 17, pb.POSITION_CONTROL, index) pb.setJointMotorControl2(self.agent, 19, pb.POSITION_CONTROL, index) pb.setJointMotorControl2(self.agent, 21, pb.POSITION_CONTROL, index) pb.setJointMotorControl2(self.agent, 24, pb.POSITION_CONTROL, middle) pb.setJointMotorControl2(self.agent, 26, pb.POSITION_CONTROL, middle) pb.setJointMotorControl2(self.agent, 28, pb.POSITION_CONTROL, middle) pb.setJointMotorControl2(self.agent, 40, pb.POSITION_CONTROL, pink) pb.setJointMotorControl2(self.agent, 42, pb.POSITION_CONTROL, pink) pb.setJointMotorControl2(self.agent, 44, pb.POSITION_CONTROL, pink) ringpos = 0.5 * (pink + middle) pb.setJointMotorControl2(self.agent, 32, pb.POSITION_CONTROL, ringpos) pb.setJointMotorControl2(self.agent, 34, pb.POSITION_CONTROL, ringpos) pb.setJointMotorControl2(self.agent, 36, pb.POSITION_CONTROL, ringpos) if self.downCameraOn: viewMatrix = down_view() else: viewMatrix = self.ahead_view() projectionMatrix = pb.computeProjectionMatrixFOV( fov, aspect, nearPlane, farPlane) w, h, img_arr, depths, mask = pb.getCameraImage( 200, 200, viewMatrix, projectionMatrix, lightDirection, lightColor, renderer=pb.ER_TINY_RENDERER) #w,h,img_arr,depths,mask = pb.getCameraImage(200,200, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pb.ER_BULLET_HARDWARE_OPENGL) #red_dimension = img_arr[:,:,0] #TODO change this so any RGB value returns 1, anything else is 0 red_dimension = img_arr[:, :, 0].flatten( ) #TODO change this so any RGB value returns 1, anything else is 0 #observation = red_dimension self.img_arr = img_arr observation = (np.absolute(red_dimension - 255) > 0).astype(int) self.current_observation = observation self.img_arr = img_arr self.depths = depths info = [42] #answer to life,TODO use real values pb.stepSimulation() self.steps += 1 #reward if moving towards the object or touching the object reward = 0 max_steps = 1000 if self.is_touching(): touch_reward = 10 if 'debug' in self.options and self.options['debug'] == True: print("TOUCHING!!!!") else: touch_reward = 0 obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify) distance = math.sqrt( sum([(xi - yi)**2 for xi, yi in zip(obj_po[0], hand_po[0]) ])) #TODO faster euclidean #distance = np.linalg.norm(obj_po[0],hand_po[0]) #print("distance:",distance) if distance < self.prev_distance: reward += 1 * (max_steps - self.steps) elif distance > self.prev_distance: reward -= 10 reward -= distance reward += touch_reward self.prev_distance = distance #print("shape",observation.shape) if 'debug' in self.options and self.options['debug'] == True: print("touch reward ", touch_reward) print("action ", action) print("reward ", reward) print("distance ", distance) if self.steps >= max_steps or self.is_touching(): done = True return observation, reward, done, info
def set_pose(body, pose): (point, quat) = pose p.resetBasePositionAndOrientation(body, point, quat, physicsClientId=CLIENT)
def set_orientation(self, orn): old_pos, _ = p.getBasePositionAndOrientation(self.body_id) p.resetBasePositionAndOrientation(self.body_id, old_pos, orn)
import pybullet as p import time p.connect(p.GUI) p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) p.setGravity(0,0,-10) arm = p.loadURDF("widowx/widowx.urdf",useFixedBase=True) p.resetBasePositionAndOrientation(arm,[-0.098612,-0.000726,-0.194018],[0.000000,0.000000,0.000000,1.000000]) while (1): p.stepSimulation() time.sleep(0.01) #p.saveWorld("test.py") viewMat = p.getDebugVisualizerCamera()[2] #projMatrix = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] projMatrix = p.getDebugVisualizerCamera()[3] width=640 height=480 img_arr = p.getCameraImage(width=width,height=height,viewMatrix=viewMat,projectionMatrix=projMatrix)
import pybullet as p import time cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True) rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0) pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0) yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0) fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0) fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0) fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0) while True: roll = p.readUserDebugParameter(rollId) pitch = p.readUserDebugParameter(pitchId) yaw = p.readUserDebugParameter(yawId) x = p.readUserDebugParameter(fwdxId) y = p.readUserDebugParameter(fwdyId) z = p.readUserDebugParameter(fwdzId) orn = p.getQuaternionFromEuler([roll, pitch, yaw]) p.resetBasePositionAndOrientation(q, [x, y, z], orn) #p.stepSimulation()#not really necessary for this demo, no physics used
lineWidth=lineWidth, lifeTime=0) pitch = 0 yaw = 0 while (p.isConnected()): pitch += 0.01 if (pitch >= 3.1415 * 2.): pitch = 0 yaw += 0.01 if (yaw >= 3.1415 * 2.): yaw = 0 baseOrientationB = p.getQuaternionFromEuler([yaw, pitch, 0]) if (obB >= 0): p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB) if (useCollisionShapeQuery): pts = p.getClosestPoints(bodyA=-1, bodyB=-1, distance=100, collisionShapeA=geom, collisionShapeB=geomBox, collisionShapePositionA=[0.5, 0, 1], collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB) #pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB) else: pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100) if len(pts) > 0:
import pybullet as p import time import pybullet_data cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True) rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0) pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0) yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0) fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0) fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0) fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0) while True: roll = p.readUserDebugParameter(rollId) pitch = p.readUserDebugParameter(pitchId) yaw = p.readUserDebugParameter(yawId) x = p.readUserDebugParameter(fwdxId) y = p.readUserDebugParameter(fwdyId) z = p.readUserDebugParameter(fwdzId) orn = p.getQuaternionFromEuler([roll, pitch, yaw]) p.resetBasePositionAndOrientation(q, [x, y, z], orn) #p.stepSimulation()#not really necessary for this demo, no physics used
def add_ball(self, x_des, y_des): self.ball1 = p.loadURDF("urdf/ball1.urdf") p.resetBasePositionAndOrientation(self.ball1, [x_des-0.3, -0.05, 0.285+y_des], (0., 0., 0.5, 0.5))
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)
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)] objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf") kuka_gripper = objects[0] print ("kuka gripper=") print(kuka_gripper) p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970]) jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ] for jointIndex in range (p.getNumJoints(kuka_gripper)): p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex]) p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0]) objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
def update_velocities(self, robot_data, velocities_dict): """Updates a given velocity command.""" pose = robot_data.pose() lin_vel = robot_data.linear_velocity() ang_vel = robot_data.angular_velocity() inv_pos, inv_rot = pb.invertTransform(pose.position, pose.quaternion) ZERO_VEC = (0, 0, 0) ZERO_ROT = (0, 0, 0, 1) if self.z_ang_joint in velocities_dict: c_ang_ib, _ = pb.multiplyTransforms(inv_pos, inv_rot, ang_vel, ZERO_ROT) d_ang_vel = max( min(velocities_dict[self.z_ang_joint], self.m_ang_v), -self.m_ang_v) ang_gain = max(min(d_ang_vel - c_ang_ib[2], self.m_ang_gain), -self.m_ang_gain) del velocities_dict[self.z_ang_joint] ang_vel, _ = pb.multiplyTransforms( ZERO_VEC, pose.quaternion, [0.0, 0.0, c_ang_ib[2] + ang_gain], ZERO_ROT) else: ang_vel = (0, 0, 0) fwd_dir, _ = pb.multiplyTransforms(ZERO_VEC, pose.quaternion, [1, 0, 0], ZERO_ROT) yaw = atan2(fwd_dir[1], fwd_dir[0]) if self.x_lin_joint in velocities_dict or self.y_lin_joint in velocities_dict: d_x_vel = 0 d_y_vel = 0 if self.x_lin_joint in velocities_dict: d_x_vel = max( min(velocities_dict[self.x_lin_joint], self.m_lin_v), -self.m_lin_v) del velocities_dict[self.x_lin_joint] if self.y_lin_joint in velocities_dict: d_y_vel = max( min(velocities_dict[self.y_lin_joint], self.m_lin_v), -self.m_lin_v) del velocities_dict[self.y_lin_joint] x_vel_gain = max(min(d_x_vel - lin_vel[0], self.m_vel_gain), -self.m_vel_gain) y_vel_gain = max(min(d_y_vel - lin_vel[1], self.m_vel_gain), -self.m_vel_gain) lin_vel = [lin_vel[0] + x_vel_gain, lin_vel[1] + y_vel_gain, 0] # lin_vel[2]] else: lin_vel = (0, 0, 0) #lin_vel[2]) new_quat = pb.getQuaternionFromEuler([0, 0, yaw]) #print(' '.join(['{}'.format(type(c)) for c in list(lin_vel) + list(ang_vel)])) # print('New position: {}\nNew velocity: {}'.format((pose.position[0], pose.position[1], robot_data.initial_pos[2]), lin_vel)) pb.resetBasePositionAndOrientation( robot_data.bId(), (pose.position[0], pose.position[1], robot_data.initial_pos[2]), new_quat, physicsClientId=robot_data.simulator.client_id()) pb.resetBaseVelocity(robot_data.bId(), lin_vel, ang_vel, physicsClientId=robot_data.simulator.client_id())