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)
