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)
basePosition, baseOrientation, linkMasses=link_Masses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=indices, linkJointTypes=jointTypes, linkJointAxis=axis, useMaximalCoordinates=useMaximalCoordinates) p.setGravity(0, 0, -10) p.setRealTimeSimulation(0) anistropicFriction = [1, 0.01, 0.01] p.changeDynamics(sphereUid, -1, lateralFriction=2, anisotropicFriction=anistropicFriction) p.getNumJoints(sphereUid) for i in range(p.getNumJoints(sphereUid)): p.getJointInfo(sphereUid, i) p.changeDynamics(sphereUid, i, lateralFriction=2, anisotropicFriction=anistropicFriction) dt = 1. / 240. SNAKE_NORMAL_PERIOD = 0.1 #1.5 m_wavePeriod = SNAKE_NORMAL_PERIOD m_waveLength = 4 m_wavePeriod = 1.5 m_waveAmplitude = 0.4
parser = urdfEditor.UrdfEditor() parser.initializeFromBulletBody(mb,physicsClientId=org) parser.saveUrdf("test.urdf") if (1): print("\ncreatingMultiBody...................n") obUid = parser.createMultiBody(physicsClientId=gui) parser2 = urdfEditor.UrdfEditor() print("\n###########################################\n") parser2.initializeFromBulletBody(obUid,physicsClientId=gui) parser2.saveUrdf("test2.urdf") for i in range (p.getNumJoints(obUid, physicsClientId=gui)): p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0,force=1,physicsClientId=gui) print(p.getJointInfo(obUid,i,physicsClientId=gui)) parser=0 p.setRealTimeSimulation(1,physicsClientId=gui) while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]): p.stepSimulation(physicsClientId=gui) time.sleep(0.01)
import pybullet as p import time import math useRealTime = 0 fixedTimeStep = 0.001 physId = p.connect(p.SHARED_MEMORY) if (physId<0): p.connect(p.GUI) p.loadURDF("plane.urdf") p.setGravity(0,0,-1) p.setTimeStep(fixedTimeStep) p.setRealTimeSimulation(0) quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,1) #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,p.JOINT_POINT2POINT,[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)
import os import sys import numpy as np import pdb from einsteinpy import coordinates clientId = p.connect(p.GUI) #choose render mode print(clientId) if (clientId < 0): #safety rendering clientId = p.connect(p.GUI) p.resetSimulation() p.setGravity(0, 0, -10) useRealTimeSim = 1 #if set to 0, stepping function must be used to step simulation p.setRealTimeSimulation(useRealTimeSim) #plane = p.loadURDF( os.path.join( pybullet_data.getDataPath(), "plane.urdf" )) #plane = p.loadURDF( os.path.join( pybullet_data.getDataPath(), "romans_urdf_files/octopus_files/octopus_simple_2_troubleshoot.urdf" ) ) #default urdf to troubleshoot is octopus_simple_2_troubleshoot.urdf plane = p.loadURDF( os.path.join( pybullet_data.getDataPath(), "romans_urdf_files/octopus_files/python_scripts_edit_urdf/output2.urdf" )) #blockPos = [0,0,3] #plane = p.loadURDF( os.path.join( pybullet_data.getDataPath(), "cube_small.urdf" ) , basePosition = blockPos )
def run(): physicsClient = p.connect( p.GUI) # p.GUI for graphical, or p.DIRECT for non-graphical version p.setAdditionalSearchPath( pybullet_data.getDataPath()) # defines the path used by p.loadURDF p.setGravity(0, 0, -10) p.setPhysicsEngineParameter(enableConeFriction=1) p.setRealTimeSimulation( 0 ) # only if this is set to 0 and with explicit steps will the torque control work correctly # load the ground plane planeId = p.loadURDF("plane.urdf") # add a box for blocked force boxStartPos = [0, -1, 1.4] boxStartOr = p.getQuaternionFromEuler([0, 0, 0]) boxId, boxConstraintId = load_constrained_urdf( "urdfs/smallSquareBox.urdf", boxStartPos, boxStartOr, physicsClient=physicsClient, ) p.changeDynamics(boxId, -1, lateralFriction=2) # load finger finger = SMContinuumManipulator(manipulator_definition) finger_startPos = [0, 1, 2.05] finger_StartOr = p.getQuaternionFromEuler([-np.pi / 2, 0, np.pi]) finger.load_to_pybullet( baseStartPos=finger_startPos, baseStartOrn=finger_StartOr, baseConstraint="static", physicsClient=physicsClient, ) p.changeDynamics(finger.bodyUniqueId, -1, lateralFriction=2) p.changeDynamics(finger.bodyUniqueId, -1, restitution=1) time_step = 0.001 p.setTimeStep(time_step) n_steps = 20000 sim_time = 0.0 lam = 1.0 omega = 1.0 torque_fns = [ lambda t: 20 * np.sin(omega * t), lambda t: 20 * np.sin(omega * t - 1 * np.pi), ] # - 0pi goes right, - pi goes left real_time = time.time() normal_forces = np.zeros((n_steps, )) normal_forces_lastLink = np.zeros((n_steps, )) time_plot = np.zeros((n_steps, )) last_link = p.getNumJoints( finger.bodyUniqueId) - 1 # last link of the finger # wait for screen recording for demo wait_for_rec = False if wait_for_rec: time.sleep(5) print("6 more seconds") time.sleep(6) for i in range(n_steps): print(torque_fns[0](sim_time)) finger.apply_actuationTorque(actuator_nr=0, axis_nr=0, actuation_torques=torque_fns[0](sim_time)) finger.apply_actuationTorque(actuator_nr=0, axis_nr=1, actuation_torques=0) p.stepSimulation() sim_time += time_step # time it took to simulate delta = time.time() - real_time real_time = time.time() # print(delta) contactPoints = p.getContactPoints(boxId, finger.bodyUniqueId, physicsClientId=physicsClient) contactPointsTip = p.getContactPoints( bodyA=boxId, bodyB=finger.bodyUniqueId, linkIndexB=last_link, physicsClientId=physicsClient, ) # print(" ---------- normal force ---------") time_plot[i] = sim_time total_normal_force = 0 for contactPoint in contactPoints: normal_force = contactPoint[9] total_normal_force += normal_force total_normal_force_lastLink = 0 print(len(contactPoints), len(contactPointsTip)) for contactPointTip in contactPointsTip: normal_forceTip = contactPointTip[9] print(normal_forceTip) total_normal_force_lastLink += normal_forceTip normal_forces[i] = total_normal_force normal_forces_lastLink[i] = total_normal_force_lastLink p.disconnect() plt.plot(time_plot, normal_forces) plt.xlabel("time") plt.ylabel("total normal force") plt.show() plt.plot(time_plot, normal_forces_lastLink) plt.xlabel("time") plt.ylabel("total normal force actuator tip") plt.show()
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 while 1: 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,
def launchSimulation( self, gui=True, use_shared_memory=False, auto_step=True): """ Launches a simulation instance Parameters: gui - Boolean, if True the simulation is launched with a GUI, and with no GUI otherwise use_shared_memory - Experimental feature, only taken into account if gui=False, False by default. If True, the simulation will use the pybullet SHARED_MEMORY_SERVER mode to create an instance. If multiple simulation instances are created, this solution allows a multicore parallelisation of the bullet motion servers (one for each instance). In DIRECT mode, such a parallelisation is not possible and the motion servers are manually stepped using the _stepSimulation method. (More information in the setup section of the qiBullet wiki, and in the pybullet documentation) auto_step - Boolean, True by default. Only taken into account if gui is False and use_shared_memory is False. If auto_step is True, the simulation is automatically stepped. Otherwise, the user will explicitely have to call @stepSimulation to step the simulation Returns: physics_client - The id of the simulation client created """ if gui: # pragma: no cover physics_client = pybullet.connect(pybullet.GUI) if auto_step: pybullet.setRealTimeSimulation( 1, physicsClientId=physics_client) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0, physicsClientId=physics_client) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0, physicsClientId=physics_client) pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0, physicsClientId=physics_client) else: if use_shared_memory: physics_client = pybullet.connect( pybullet.SHARED_MEMORY_SERVER) if auto_step: pybullet.setRealTimeSimulation( 1, physicsClientId=physics_client) else: physics_client = pybullet.connect(pybullet.DIRECT) if auto_step: threading.Thread( target=self._stepSimulation, args=[physics_client]).start() self.setGravity(physics_client, [0.0, 0.0, -9.81]) return physics_client
p.createSoftBodyAnchor(armId3, 8, mod3, -1) p.createSoftBodyAnchor(armId3, 16, mod3, -1) p.createSoftBodyAnchor(armId3, 17, mod3, -1) p.createSoftBodyAnchor(armId4, 7, mod4, -1) p.createSoftBodyAnchor(armId4, 8, mod4, -1) p.createSoftBodyAnchor(armId4, 16, mod4, -1) p.createSoftBodyAnchor(armId4, 17, mod4, -1) numLinks = p.getMeshData(armId1) pos, ort = p.getBasePositionAndOrientation(armId1) # run simulation useRealTimeSimulation = 0 if (useRealTimeSimulation): p.setRealTimeSimulation(0) posArr1 = [] posArr2 = [] posArr3 = [] posArr4 = [] posArr5 = [] velArr1 = [] velArr2 = [] velArr3 = [] velArr4 = [] mods = [mod1, mod2, mod3, mod4] # Control related variables
def __init__(self, use_interactive=True, is_interactive_realtime=True, hide_ui=True, topdown_viewport=False, log_dir=None, log_bullet_states=False, log_mp4=False, robot_skew=0.0): """Sets up simulation elements. Two sets of preferences affect how the simulation behaves. Choosing a interactive simulation session allows you to interact with the robot through keyboard and mouse. Headless sessions run much quicker and can be used for automated testing. Logging helps narrow down how and when the robot fails. A mp4 video from the viewport and/or .bullet restorable states can be saved to the log folder. :param use_interactive: determines whether the pybullet simulation runs with a GUI open or headless. :param is_interactive_realtime: when interactive, choose manual timestepping or run realtime. headless sim only uses manual timestepping. :param hide_ui: when interactive, choose to hide UI elements. :param topdown_viewport: when interactive, choose between default and topdown viewports. :param log_dir: the directory in which to log. :param log_bullet_states: logs .bullet sim states every 5.0 sec or every 1200 iterations. :param log_mp4: when interactive, logs .mp4 video until sim completes via a graceful stop (corrupts log if sim ends ungracefully). :param robot_skew: [-inf, inf] where negative values skew left, 0 is no skew, and positive skew right """ self.use_interactive = use_interactive self.is_interactive_realtime = is_interactive_realtime self.hide_ui = hide_ui self.topdown_viewport = topdown_viewport self.log_dir = log_dir self.log_bullet_states = log_bullet_states self.log_mp4 = log_mp4 self.TIMESTEPPING_DT = 1 / 240 self.PRESSED_THRES = -.0038 p.connect(p.GUI if self.use_interactive else p.DIRECT) p.resetSimulation() p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0 if self.hide_ui else 1) if self.topdown_viewport: p.resetDebugVisualizerCamera(1.1, 0.0, -89.9, (0.0, 0.0, 0.0)) else: p.resetDebugVisualizerCamera(2, 30.0, -50.0, (0.0, 0.2, 0.0)) p.setGravity(0, 0, -9.8) if self.log_mp4: p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, os.join(self.log_dir, "log.mp4")) p.setRealTimeSimulation(1 if self.is_interactive_realtime else 0) self.mobile_agent = BlockStackerAgent(skew=robot_skew) self.field = Field() self.legos = Legos()
def velocity(self, value): Actuator.setVelocity(self.joint, value) @property def torque(self): jointState = self.joint.getState() return jointState.appliedTorque @torque.setter def torque(self, value): Actuator.setTorque(self.joint, value) if __name__ == "__main__": import os sid = pb.connect(pb.GUI) cwd = os.getcwd() pb.setAdditionalSearchPath(cwd + "../../data") pb.setGravity(0, 0, -9.8, sid) robot = BaxterRobot() robot.resetJoints() pb.setRealTimeSimulation(True) for i in range(1000000): pb.stepSimulation() robot.controlActuators({"left_e0": 1.0}, controlType="torque") print(robot.actuators.keys()) print(robot.joints.keys())
def __init__(self, config): """ :param config: dict """ if self.seed is not None: # Set random seed from config self.seed(config["seed"]) else: # Set seed randomly from current time rnd_seed = int((time.time() % 1) * 10000000) np.random.seed(rnd_seed) T.manual_seed(rnd_seed + 1) self.config = config # (x_delta, y_delta, z_delta), (qx,qy,qz,qw), (x_vel,y_vel,z_vel), (x_ang_vel, y_ang_vel, z_ang_vel) self.raw_obs_dim = 13 # Specify what you want your observation to consist of (how many past observations of each type) self.obs_dim = self.config["obs_input"] * self.raw_obs_dim \ + self.config["act_input"] * 4 \ + self.config["rew_input"] * 1 \ + self.config["latent_input"] * 7 \ + self.config["step_counter"] * 1 self.act_dim = 4 self.reactive_torque_dir_vec = [1, -1, -1, 1] # Episode variables self.step_ctr = 0 self.episode_ctr = 0 # Velocity [0,1] which the motors are currently turning at (unobservable) self.current_motor_velocity_vec = np.array([0., 0., 0., 0.]) if (config["animate"]): self.client_ID = p.connect(p.GUI) else: self.client_ID = p.connect(p.DIRECT) # Set simulation parameters p.setGravity(0, 0, -9.8, physicsClientId=self.client_ID) p.setRealTimeSimulation(0, physicsClientId=self.client_ID) p.setTimeStep(self.config["sim_timestep"], physicsClientId=self.client_ID) p.setAdditionalSearchPath(pybullet_data.getDataPath(), physicsClientId=self.client_ID) # Instantiate instances of joystick controller (human input) and pid controller self.joystick_controller = JoyController(self.config) self.pid_controller = PIDController(self.config) # Load robot model and floor self.robot, self.plane = self.load_robot() # Define observation and action space (for gym) self.observation_space = spaces.Box( low=-self.config["observation_bnd"], high=self.config["observation_bnd"], shape=(self.obs_dim, )) self.action_space = spaces.Box(low=-1, high=1, shape=(self.act_dim, )) # Temporally correlated noise for disturbances self.rnd_target_vel_source = my_utils.SimplexNoise(4, 15) self.obs_queue = [ np.zeros(self.raw_obs_dim, dtype=np.float32) for _ in range( np.maximum(1, self.config["obs_input"]) + self.randomized_params["input_transport_delay"]) ] self.act_queue = [ np.zeros(self.act_dim, dtype=np.float32) for _ in range( np.maximum(1, self.config["act_input"]) + self.randomized_params["output_transport_delay"]) ] self.rew_queue = [ np.zeros(1, dtype=np.float32) for _ in range( np.maximum(1, self.config["rew_input"]) + self.randomized_params["input_transport_delay"]) ] self.create_targets()
p.setCollisionFilterPair(bodyUniqueIdA=pendulum_uniqueId_pybullet, bodyUniqueIdB=pendulum_uniqueId_z3, linkIndexA=0, linkIndexB=-1, enableCollision=0) p.setCollisionFilterPair(bodyUniqueIdA=pendulum_uniqueId_pybullet, bodyUniqueIdB=pendulum_uniqueId_z3, linkIndexA=-1, linkIndexB=0, enableCollision=0) # enables gravity p.setGravity(0, 0, -9.8) # sets real time simulation p.setRealTimeSimulation( enableRealTimeSimulation=0 ) # now we dont have to call p.stepSimulation() in order to advance the time step of the simulation environment # turn off all motors so that joints are not stiffened for the rest of the simulations p.setJointMotorControlArray(bodyUniqueId=pendulum_uniqueId_pybullet, jointIndices=list(range(number_of_links_urdf)), controlMode=p.TORQUE_CONTROL, positionGains=[0.1] * number_of_links_urdf, velocityGains=[0.1] * number_of_links_urdf, forces=[0] * number_of_links_urdf) # load the block thatwe are trying to avoid targetPositions = [0] * (number_of_links_urdf) targetPositions[0] = 3.14 * (36 / 360) # set desired joint angles in radians targetVelocities = [0] * (number_of_links_urdf) # set desired joint velocities
def __init__(self): rospy.loginfo("Subscribing to /tf topic...") self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) # set the parameters self.env_sdf_file_path = rospy.get_param("~env_sdf_file_path", "") self.robot_urdf_file_path = rospy.get_param("~robot_urdf_file_path", "r2d2.urdf") self.base_frame_id = rospy.get_param("~base_frame_id", "base_link") self.global_frame_id = rospy.get_param("~global_frame_id", "map") self.objects_cad_models_dir = rospy.get_param( "~objects_cad_models_dir", "") self.robot_cad_models_dir = rospy.get_param("~robot_cad_models_dir", "") self.position_tolerance = rospy.get_param("~position_tolerance", 0.01) self.velocity_tolerance = rospy.get_param("~velocity_tolerance", 0.001) self.camera_info_topic = rospy.get_param("~rgb_camera_info_topic", "/camera/rgb/camera_info") self.time_step = rospy.get_param("~time_step", 1 / 240.0) # Initialize attributes self.simulator_entity_id_map = {} self.reverse_simulator_entity_id_map = {} self.simulator_joint_id_map = {} self.reverse_simulator_joint_id_map = {} self.entities_state = {} self.entities_last_update = {} self.camera_frame_id = None self.last_backup_id = None self.camera_info = None self.last_observation_time = None self.average_observation_duration = None self.bridge = CvBridge() self.visualization_publisher = rospy.Publisher( "uwds3_core/debug_image", sensor_msgs.msg.Image, queue_size=1) # Setup the physics server use_gui = rospy.get_param("~use_gui", True) if use_gui is True: self.physics = p.connect(p.GUI) else: self.physics = p.connect(p.DIRECT) if p.isNumpyEnabled() is not True: rospy.logwarn("Numpy is not enabled in pybullet !") # Add search path p.setAdditionalSearchPath(pybullet_data.getDataPath()) # Load the ground self.simulator_entity_id_map[self.global_frame_id] = p.loadURDF( "plane.urdf") # if self.robot_cad_models_dir != "": # p.setAdditionalSearchPath(self.robot_cad_models_dir) if self.objects_cad_models_dir != "": p.setAdditionalSearchPath(self.objects_cad_models_dir) # Load the environment if any if self.env_sdf_file_path != "": self.simulator_entity_id_map["env"] = p.loadURDF( self.env_sdf_file_path) self.robot_loaded = False # Set the gravity p.setGravity(0, 0, -10) p.setRealTimeSimulation(0) # TODO #self.simulation_timer = # Subscribe to joint state message rospy.loginfo("Subscribing to /joint_states topic...") self.joint_state_subscriber = rospy.Subscriber( "/joint_states", sensor_msgs.msg.JointState, self.joint_states_callback) self.camera_info_topic = rospy.get_param("~camera_info_topic", "/camera/rgb/camera_info") rospy.loginfo("Subscribing to /{} topic...".format( self.camera_info_topic)) self.camera_info_received = False self.camera_info_subscriber = rospy.Subscriber( self.camera_info_topic, sensor_msgs.msg.CameraInfo, self.camera_info_callback)
import pybullet as p import time cid = p.connect(p.SHARED_MEMORY) if (cid<0): p.connect(p.GUI) p.resetSimulation() p.setGravity(0,0,-10) useRealTimeSim = 1 #for video recording (works best on Mac and Linux, not well on Windows) #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") p.setRealTimeSimulation(useRealTimeSim) # either this p.loadURDF("plane.urdf") #p.loadSDF("stadium.sdf") car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True) for i in range (p.getNumJoints(car)): print (p.getJointInfo(car,i)) for wheel in range(p.getNumJoints(car)): p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0) p.getJointInfo(car,wheel) wheels = [8,15] print("----------------") #p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10) c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c,gearRatio=1, maxForce=10000)
rangex = 32 rangey = 32 for i in range(rangex): for j in range(rangey): p.createMultiBody(baseMass=1, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[((-rangex / 2) + i) * meshScale[0] * 2, (-rangey / 2 + j) * meshScale[1] * 2, 1], useMaximalCoordinates=True) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.stopStateLogging(logId) p.setGravity(0, 0, -10) p.setRealTimeSimulation(1) colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]] currentColor = 0 while (1): mouseEvents = p.getMouseEvents() for e in mouseEvents: if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)): mouseX = e[1] mouseY = e[2] rayFrom, rayTo = getRayFromTo(mouseX, mouseY) rayInfo = p.rayTest(rayFrom, rayTo) #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3) for l in range(len(rayInfo)): hit = rayInfo[l]
p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000]) objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)] objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] ob = objects[0] jointPositions=[ 0.000000 ] for jointIndex in range (p.getNumJoints(ob)): p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)] ob = objects[0] jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ] for jointIndex in range (p.getNumJoints(ob)): p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.setGravity(0.000000,0.000000,0.000000) p.setGravity(0,0,-10) ##show this for 10 seconds #now = time.time() #while (time.time() < now+10): # p.stepSimulation() p.setRealTimeSimulation(1) while (1): p.setGravity(0,0,-10) p.disconnect()
import pybullet import pybullet_data import time import os pybullet.connect(pybullet.GUI) pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) robot = pybullet.loadURDF( os.path.expanduser('~') + '/repos_cloned/kuka_experimental/kuka_kr210_support/urdf/kr210l150.urdf', useFixedBase=1) pybullet.setGravity(0, 0, -9.81) plane = pybullet.loadURDF('plane.urdf') pybullet.setRealTimeSimulation(0) pybullet.setJointMotorControlArray(robot, range(6), pybullet.POSITION_CONTROL, targetPositions=[0.5] * 6) # pybullet.setJointMotorControlArray(robot, range(6), pybullet.TORQUE_CONTROL, forces=[90000] * 6) for _ in range(300): pybullet.stepSimulation() time.sleep(0.01)