def _reset(self):
# reset state
self.steps = 0
self.done = False
# reset pole on cart in starting poses
p.resetBasePositionAndOrientation(self.cart, (0, 0, 0.08),
(0, 0, 0, 1))
p.resetBasePositionAndOrientation(self.pole, (0, 0, 0.35),
(0, 0, 0, 1))
for _ in xrange(100):
p.stepSimulation()
# give a fixed force push in a random direction to get things going...
theta = np.multiply(
np.multiply((np.random.random(), np.random.random(), 0), 2) -
(1, 1, 0), 5) if self.random_theta else (1, 0, 0)
for _ in xrange(self.initial_force_steps):
p.stepSimulation()
p.applyExternalForce(self.pole, -1, theta, (0, 0, 0),
p.WORLD_FRAME)
if self.delay > 0:
time.sleep(self.delay)
# bootstrap state by running for all repeats
for i in xrange(self.repeats):
self.set_state_element_for_repeat(i)
# return this state
return np.copy(self.state)
def check_grip(cubeID, handID):
# TODO: make direction of motion consistant (up and away from origin?)
# TODO: modify to take in hand and cube position/orientation + load into environment w/gravity before shaking
"""
check grip by adding in gravity
"""
print("checking strength of current grip")
mag = 1
pos, oren = p.getBasePositionAndOrientation(handID)
# pos, oren = p.getBasePositionAndOrientation(cubeID)
time_limit = .5
finish_time = time() + time_limit
p.addUserDebugText("Grav Check!", [-.07, .07, .07], textColorRGB=[0, 0, 1], textSize=1)
while time() < finish_time:
p.stepSimulation()
# add in "gravity"
p.applyExternalForce(cubeID, linkIndex=-1, forceObj=[0, 0, -mag], posObj=pos, flags=p.WORLD_FRAME)
contact = p.getContactPoints(cubeID, handID) # see if hand is still holding obj after gravity is applied
print("contacts", contact)
if len(contact) > 0:
p.removeAllUserDebugItems()
p.addUserDebugText("Grav Check Passed!", [-.07, .07, .07], textColorRGB=[0, 1, 0], textSize=1)
sleep(.3)
print("Good Grip to Add")
return get_robot_config(handID)
else:
p.removeAllUserDebugItems()
p.addUserDebugText("Grav Check Failed!", [-.07, .07, .07], textColorRGB=[1, 0, 0], textSize=1)
sleep(.3)
return None
def applyAction(angle, offset):
cylPos, cylOrn = p.getBasePositionAndOrientation(cylId)
cubePos, cubeOrn = p.getBasePositionAndOrientation(cubeId)
cubeNewPos = [
cylPos[0] + math.cos(math.radians(angle)),
cylPos[1] + math.sin(math.radians(angle)), 0
]
vec = np.array([cylPos[0] - cubeNewPos[0], cylPos[1] - cubeNewPos[1], 0])
vec = vec / np.linalg.norm(vec)
look = [0, 0, math.atan(vec[0] / (-vec[1]))]
cubeNewPosWithOffset = cubeNewPos
cubeNewPosWithOffset[0] -= offset * math.sin(math.radians(angle))
cubeNewPosWithOffset[1] += offset * math.cos(math.radians(angle))
p.resetBasePositionAndOrientation(cubeId, cubeNewPosWithOffset,
p.getQuaternionFromEuler(look))
for i in range(100):
eachIter()
for i in range(100):
p.applyExternalForce(cubeId,
-1,
20 * np.array(vec),
cubeNewPos,
flags=p.WORLD_FRAME)
eachIter()
for i in range(400):
eachIter()
cubePos, cubeOrn = p.getBasePositionAndOrientation(cubeId)
print np.array(list(cubePos)) - cubeNewPosWithOffset
def apply_external_force(self, k, start, duration, F, M):
"""Apply an external force/momentum to the robot
4-th order polynomial: zero force and force velocity at start and end
(bell-like force trajectory)
Args:
k (int): numero of the current iteration of the simulation
start (int): numero of the iteration for which the force should start to be applied
duration (int): number of iterations the force should last
F (3x array): components of the force in PyBullet world frame
M (3x array): components of the force momentum in PyBullet world frame
"""
if ((k < start) or (k > (start+duration))):
return 0.0
"""if k == start:
print("Applying [", F[0], ", ", F[1], ", ", F[2], "]")"""
ev = k - start
t1 = duration
A4 = 16 / (t1**4)
A3 = - 2 * t1 * A4
A2 = t1**2 * A4
alpha = A2*ev**2 + A3*ev**3 + A4*ev**4
self.applied_force[:] = alpha * F
pyb.applyExternalForce(self.robotId, -1, alpha * F, alpha*M, pyb.LINK_FRAME)
return 0.0
def step(self, action):
self.quadruped_pos, self.quadruped_orientation = p.getBasePositionAndOrientation(
self.quadruped_id)
action = np.clip(np.asarray(action[:]), self.action_space.low,
self.action_space.high)
self._perform_action(action)
p.stepSimulation()
self.prev_action = action
if self.env_step_counter % 200 == 0:
external_force = ((np.random.rand(3) * 2) - 1) * 15000
external_force[2] = external_force[2] / 4
p.applyExternalForce(self.quadruped_id, -1, external_force,
[0, 0, 0], p.LINK_FRAME)
p.stepSimulation()
self._observation = self._compute_observation()
reward = self._compute_reward()
done = self._compute_done()
self.env_step_counter += 1
return np.array(self._observation), reward, done, {}
def apply_push_force(self, x, y, force):
"""
Apply pushing force to a 3D point. Given a pixel location (x, y),
compute the 3D location of that point, and then apply a virtual force
of a given magnitude towards the negative surface normal at that point
:param x: image pixel x coordinate
:param y: image pixel y coordinate
:param force: force magnitude
"""
camera_pose = np.array([self.px, self.py, self.pz])
self.renderer.set_camera(camera_pose,
camera_pose + self.view_direction, self.up)
position_cam = np.array([
(x - self.renderer.width / 2) / float(self.renderer.width / 2) *
np.tan(self.renderer.horizontal_fov / 2.0 / 180.0 * np.pi),
-(y - self.renderer.height / 2) / float(self.renderer.height / 2) *
np.tan(self.renderer.vertical_fov / 2.0 / 180.0 * np.pi), -1, 1
])
position_cam[:3] *= 5
position_world = np.linalg.inv(self.renderer.V).dot(position_cam)
position_eye = camera_pose
res = p.rayTest(position_eye, position_world[:3])
if len(res) > 0 and res[0][0] != -1:
# there is hit
object_id, link_id, _, hit_pos, hit_normal = res[0]
p.changeDynamics(object_id,
-1,
activationState=p.ACTIVATION_STATE_WAKE_UP)
p.applyExternalForce(object_id, link_id,
-np.array(hit_normal) * force, hit_pos,
p.WORLD_FRAME)
def add_perturb_force(self):
perturb_force_norm = self.adr.value(
"max_perturb_force") * np.random.random()
found = False
while not found:
perturb_force = np.random.normal(size=3)
norm_2 = np.sum(np.square(perturb_force))
if norm_2 >= 1e-5:
found = True
perturb_force = perturb_force * perturb_force_norm / np.sqrt(
norm_2)
if self.state.frame % 30 == 0:
perturb_force = [
perturb_force[i] + self.state.offset_force[i] for i in range(3)
]
else:
perturb_force = [perturb_force[i] for i in range(3)]
p.applyExternalForce(self.robotId,
-1,
perturb_force, [0, 0, 0],
p.LINK_FRAME,
physicsClientId=self.pcId)
def step(self, t=None):
"""
does one step in the simulation
"""
# try to stay real-time
if self.display:
if t is None:
time.sleep(self.dt)
else:
time_to_wait = t - time.time()
while time_to_wait > 0:
time.sleep(0.9 * time_to_wait)
time_to_wait = t - time.time()
# apply external force to keep spacecraft position fixed
pos, ori = p.getBasePositionAndOrientation(self.robot_id)
vel = p.getBaseVelocity(self.robot_id)
f = - 150. * np.array(pos) - 50. * np.array(vel[0])
p.applyExternalForce(self.robot_id, -1, f, pos, p.WORLD_FRAME)
# reset shooting star
if self.shootingstar:
pos, ori = p.getBasePositionAndOrientation(self.shot_id)
if np.linalg.norm(np.array(pos)) > 15:
self.place_shootingstar()
# take a simulation step
p.stepSimulation()
def run_trial(trial_num, object_id, force, mass, friction, verbose=True):
change_object_dynamics(object_id, mass, friction)
start_x = p.getBasePositionAndOrientation(object_id)[0][0]
if verbose:
print('\n*************TRIAL %d**************' % trial_num)
print_log(object_id, force)
# log1_id = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
# 'data/pybullet_logs/log_object_dynamics_%s.bin' % str(trial_num).zfill(5))
# log2_id = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS,
# 'data/pybullet_logs/log_contact_dynamics_%s.bin' % str(trial_num).zfill(5))
p.applyExternalForce(object_id,
-1,
force,
posObj=[0, 0, 0],
flags=p.LINK_FRAME)
p.stepSimulation()
while is_object_moving(object_id):
p.stepSimulation()
time.sleep(1 / 240)
return p.getBasePositionAndOrientation(
object_id)[0][0] - start_x # return total x-displacement
def up_force(obj, mag):
"""Upward force on obj of a given magnitude mag."""
p.applyExternalForce(objectUniqueId=obj,
linkIndex=-1,
forceObj=[0, 0, mag],
posObj=[0, 0, 0],
flags=p.LINK_FRAME)
def apply_tcp_force(self, force):
p.applyExternalForce(self.kuka_uid,
self.ee_index,
force,
[0,0,0],
p.WORLD_FRAME,
self.client)
def update(self, tentacleMask=None):
if tentacleMask is None:
tentacleMask = [True] * len(self.tentacles)
ends = [p.getBasePositionAndOrientation(tentacle.particles[-1])[0] for tentacle in self.tentacles]
for tentacle, end, offset, maskVal in zip(self.tentacles, ends, self.offsets, tentacleMask):
if not maskVal:
continue
diff = np.subtract(np.array(ends)[tentacleMask], end)
norm = np.linalg.norm(diff, axis=1)
diff = diff[norm != 0]
norm = norm[norm != 0]
if len(norm > 0):
idx = [np.argmin(norm)]
direction = diff[idx] / norm[idx]
axis = np.subtract(end, p.getBasePositionAndOrientation(tentacle.particles[-2])[0])
axis = axis / np.linalg.norm(axis)
self.x = (self.x + np.random.standard_normal())
self.y = (self.y + np.random.standard_normal())
x, y = [int(self.x + offset[0]) % self.perlin.shape[0], int(self.y + offset[1]) % self.perlin.shape[0]]
# print(x,y)
angle = np.sign(self.perlin[x, y]) * 1
rot = axis_angle_to_mat(axis, angle)
direction = np.dot(rot, direction.T)
direction = direction / np.linalg.norm(direction) ** 2
# print(direction)
p.applyExternalForce(tentacle.particles[-1], -1, direction * self.intensity, end, p.WORLD_FRAME)
def bump_cart(self, a_cart):
p.applyExternalForce(
a_cart,
-1, # LINK_INDEX
self.random_force_in_plane(),
(0, 0, 0),
p.LINK_FRAME)
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)
force_x = self.np_random.uniform(-300, 300)
force_y = self.np_random.uniform(-300, 300)
more_compact = 0.5 # set to 1.0 whole football field
#self.walk_target_x *= more_compact
#self.walk_target_y *= more_compact
startx, starty, _ = self.robot.get_position()
self.flag = None
#self.flag = self.scene.cpp_world.debug_sphere(self.walk_target_x, self.walk_target_y, 0.2, 0.2, 0xFF8080)
self.flag_timeout = 3000 / self.scene.frame_skip
#print('targetxy', self.flagid, self.walk_target_x, self.walk_target_y, p.getBasePositionAndOrientation(self.flagid))
#p.resetBasePositionAndOrientation(self.flagid, posObj = [self.walk_target_x, self.walk_target_y, 0.5], ornObj = [0,0,0,0])
if self.lastid:
p.removeBody(self.lastid)
self.lastid = p.createMultiBody(
baseMass=1,
baseVisualShapeIndex=self.visualid,
baseCollisionShapeIndex=self.colisionid,
basePosition=[startx, starty, 0.5])
p.applyExternalForce(self.lastid, -1, [force_x, force_y, 50],
[0, 0, 0], p.LINK_FRAME)
ball_xyz, _ = p.getBasePositionAndOrientation(self.lastid)
self.robot.walk_target_x = ball_xyz[0]
self.robot.walk_target_y = ball_xyz[1]
def apply_thrust(self, thrust):
if self.fuel > 0.0:
thrust = np.min([self.fuel / self.kg_per_kN, thrust])
# apply thrust to the axis of the rocket's bell nozzle
p.applyExternalForce(self.bot_id, -1, [0.0, 0.0, thrust],\
posObj=[0,0,1.0],flags=p.LINK_FRAME)
# decrement fuel proportional to the thrust used
self.fuel -= thrust * self.kg_per_kN
p.changeDynamics(self.bot_id, 0, mass=self.fuel+self.dry_weight)
self.fuel = 0.0 if self.fuel <= 0.0 else self.fuel
else:
thrust = 0.0
if "balance" in self.mode:
thrust = 0
if self.render and thrust:
# visual indicators of thrust
p.changeVisualShape(self.bot_id, -1, rgbaColor=[5e-4*thrust, 0.01, 0.01, 1.0])
if thrust:
p.changeVisualShape(self.bot_id, 10, rgbaColor=[\
1.0, 0.1, 0.0, thrust/self.max_thrust[2]/2])
else:
p.changeVisualShape(self.bot_id, 10, rgbaColor=[0.,0.,0.,0.])
def _reset(self):
# reset state
self.steps = 0
self.done = False
# reset pole on cart in starting poses
p.resetBasePositionAndOrientation(
self.cartpole, (0, 0, 0.08),
(0, 0, 0, 1)) # reset cart position and orientation
p.resetJointState(self.cartpole, 0, 0) # reset joint position of pole
for _ in xrange(100):
p.stepSimulation()
# give a fixed force push in a random direction to get things going...
theta = (np.random.random() * 2 - 1) if self.random_theta else 0.0
for _ in xrange(self.initial_force_steps):
p.stepSimulation()
p.applyExternalForce(self.cartpole, 0, (theta, 0, 0), (0, 0, 0),
p.WORLD_FRAME)
if self.delay > 0:
time.sleep(self.delay)
# bootstrap state by running for all repeats
for i in xrange(self.repeats):
self.set_state_element_for_repeat(i)
# return this state
return np.copy(self.state)
def test_force_gains(gains):
c = CubePD(force_gains=gains)
obs = env.reset()
cube = env.unwrapped.platform.cube
p.resetDebugVisualizerCamera(cameraDistance=0.6,
cameraYaw=0,
cameraPitch=-40,
cameraTargetPosition=[0, 0, 0])
robot_pos = obs['robot_position']
obs['object_position'] = np.array([0, 0, 0.0325])
cube.set_state(obs['object_position'], obs['object_orientation'])
goal = np.array([0, 0, 0.135])
goal_ori = np.array([0, 0, 0, 1])
total_error = 0
for _ in range(1000):
force, _ = c(goal, goal_ori, obs['object_position'],
obs['object_orientation'])
p.applyExternalForce(objectUniqueId=cube.block,
linkIndex=-1,
forceObj=force,
posObj=np.zeros(3),
flags=p.LINK_FRAME)
obs, _, _, _ = env.step(robot_pos)
total_error += np.linalg.norm(goal - obs['object_position'])
# time.sleep(0.01)
return total_error
def timer_callback(_obj, _event):
cnt = next(counter)
# tb.message = "Let's count up to 100 and exit :" + str(cnt)
# showm.scene.azimuth(0.05 * cnt)
# sphere_actor.GetProperty().SetOpacity(cnt/100.)
showm.render()
# print(sphere_actor.GetPosition())
pos, orn = p.getBasePositionAndOrientation(obj)
if storage.f:
# time.sleep(3)
print("entered")
for j in range(5):
p.applyExternalForce(obj,
-1,
forceObj=[-1000, 0, 0],
posObj=pos,
flags=p.WORLD_FRAME)
_, orn = p.getBasePositionAndOrientation(obj)
storage.f = 0
cuboid_actor.SetPosition(pos[0], pos[1], pos[2])
orn_deg = np.degrees(p.getEulerFromQuaternion(orn))
cuboid_actor.SetOrientation(orn_deg[0], orn_deg[1], orn_deg[2])
# cuboid_actor.RotateWXYZ(orn[1], orn[2], orn[3], orn[0])
p.stepSimulation()
print(orn)
storage.orn_prev = orn
if cnt == 2000:
showm.exit()
def test_pd_controller():
c = CubePD()
cube = env.unwrapped.platform.cube
obs = env.reset()
p.resetDebugVisualizerCamera(cameraDistance=0.6,
cameraYaw=0,
cameraPitch=-40,
cameraTargetPosition=[0, 0, 0])
robot_pos = obs['robot_position']
obs['object_position'] = np.array([0, 0, 0.0325])
goal_pos = obs['goal_object_position']
goal_ori = obs['goal_object_orientation']
done = False
while not done:
force, torque = c(goal_pos, goal_ori, obs['object_position'],
obs['object_orientation'])
p.applyExternalForce(objectUniqueId=cube.block,
linkIndex=-1,
forceObj=force,
posObj=np.zeros(3),
flags=p.LINK_FRAME)
p.applyExternalTorque(objectUniqueId=cube.block,
linkIndex=-1,
torqueObj=torque,
flags=p.LINK_FRAME)
obs, _, done, _ = env.step(robot_pos)
time.sleep(0.01)
def throwBall(self):
robotPos, robotOrn = p.getBasePositionAndOrientation(
self.robot, physicsClientId=self.physicsClientId)
angle = random.uniform(0, 2) * math.pi
throwHeight = random.uniform(0.1, 0.5)
position = [
robotPos[0] + math.cos(angle) *
(self.throwBallDistance + random.uniform(-0.1, 0.1)),
robotPos[1] + math.sin(angle) *
(self.throwBallDistance + random.uniform(-0.1, 0.1)),
robotPos[2] + throwHeight
]
p.resetBasePositionAndOrientation(self.ball,
position,
p.getQuaternionFromEuler([0, 0, 0]),
physicsClientId=self.physicsClientId)
force = random.uniform(0.5, 1.0) * self.throwBallForce
p.applyExternalForce(
self.ball,
-1, [
-math.cos(angle) * force, -math.sin(angle) * force,
(random.uniform(
(0.15 - throwHeight) * self.throwBallDistance,
(0.65 - throwHeight) * self.throwBallDistance)) * force
], [0, 0, 0],
p.LINK_FRAME,
physicsClientId=self.physicsClientId)
def timer_callback(_obj, _event):
global apply_force, fpss
cnt = next(counter)
showm.render()
if cnt % 1 == 0:
fps = scene.frame_rate
fpss = np.append(fpss, fps)
tb.message = "Avg. FPS: " + str(np.round(np.mean(fpss), 0)) + \
"\nSim Steps: " + str(cnt)
# Get the position and orientation of the first domino.
domino1_pos, domino1_orn = p.getBasePositionAndOrientation(dominos[0])
# Apply force on the First Domino (domino) above the Center of Mass.
if apply_force:
# Apply the force.
p.applyExternalForce(dominos[0],
-1,
forceObj=[100, 0, 0],
posObj=domino1_pos + np.array([0, 0, 1.7]),
flags=p.WORLD_FRAME)
apply_force = False
# Updating the position and orientation of individual dominos.
for idx, domino in enumerate(dominos):
sync_domino(idx, domino)
utils.update_actor(domino_actor)
# Simulate a step.
p.stepSimulation()
# Exit after 300 steps of simulation.
if cnt == 300:
showm.exit()
def __onClickExcute(self):
if not self.firstClick:
p.changeDynamics(self.colId, -1, 10)
self.base_rotate = [0, 0, 1, 0]
self.col_local_pos = [0, 0, 0]
self.__ChangeStatus(STATUS_FLY)
currentScene = SceneManager.GetCurrentScene()
multi = currentScene.speedButton.GetCurrentTickZone()
currentScene.SetState("STATE_PLAYING")
currentScene.pauseButton.Display()
self.firstClick = True
data = self.GetData()
if data:
farMul, highMul = data['baseFarMultiply'], data[
'baseHighMultiply']
else:
farMul, highMul = 1.0, 1.0
farMul = farMul * multi
highMul = highMul * 0.5 * multi
pos, orn = p.getBasePositionAndOrientation(self.colId)
force = [0, -self.camDis[1] * 10000 * farMul, 20000 * highMul]
p.applyExternalForce(self.colId,
-1,
force,
pos,
flags=p.WORLD_FRAME)
def control_prop(modArray, curLocs, desiredLocs, c=1):
if len(curLocs.shape) == 3:
curLocs = curLocs[0]
for i in range(len(modArray)):
p.applyExternalForce(modArray[i], -1,
c * (desiredLocs[i] - curLocs[i]), [0, 0, 0],
p.LINK_FRAME)
def apply_external_disturbances(self):
#Apply external disturbance force
if np.random.rand() < self.config["disturbance_frequency"]:
self.current_disturbance = {
"vector":
np.array([
2 * np.random.rand() - 1.0, 2 * np.random.rand() - 1.0,
0.4 * np.random.rand() - 0.2
]),
"remaining_life":
np.random.randint(10, 40),
"effect":
np.random.choice(["translation", "rotation"])
}
#self.current_disturbance["visual_shape"] = p.createVisualShape()
if self.current_disturbance is None: return
if self.current_disturbance["effect"] == "translation":
p.applyExternalForce(self.robot,
linkIndex=-1,
forceObj=self.current_disturbance["vector"] *
self.config["disturbance_intensity"],
posObj=[0, 0, 0],
flags=p.LINK_FRAME)
else:
p.applyExternalTorque(
self.robot,
linkIndex=-1,
torqueObj=self.current_disturbance["vector"] *
self.config["disturbance_intensity"] * 0.15,
flags=p.LINK_FRAME)
if self.current_disturbance is not None:
self.current_disturbance["remaining_life"] -= 1
if self.current_disturbance["remaining_life"] <= 0:
#p.removeBody(self.current_disturbance["visual_shape"])
self.current_disturbance = None
def _launchBall(self):
if self.ball_launched:
return
# print("Ball launched!!")
shooter_state = self._getDroneStateVector(self.shooter_id)
R_b2gs = np.array(pb.getMatrixFromQuaternion(
shooter_state[3:7])).reshape((3, 3))
ball_pos = 1.5 * R_b2gs[:, 0] * self.L + shooter_state[0:3]
self.ball_id = pb.loadURDF("sphere2.urdf",
ball_pos,
pb.getQuaternionFromEuler([0, 0, 0]),
globalScaling=0.0625,
physicsClientId=self.CLIENT)
pb.changeDynamics(self.ball_id, -1, mass=0.1)
# shooter_vel, _ = pb.getBaseVelocity(
# self.shooter_id,
# physicsClientId=self.CLIENT
# )
# # print("Shooter vel:", shooter_vel)
# shooter_vel = np.array(shooter_vel)
# ball_force_unit = shooter_vel/(np.linalg.norm(shooter_vel) + 1e-6)
ball_force = self.space_multiplier * 115 * R_b2gs[:, 0]
# print("Force on ball:", ball_force)
pb.applyExternalForce(self.ball_id,
-1,
forceObj=ball_force.tolist(),
posObj=[0, 0, 0],
flags=pb.LINK_FRAME,
physicsClientId=self.CLIENT)
self.ball_launched = True
def timer_callback(_obj, _event):
global apply_force
cnt = next(counter)
showm.render()
red_pos, red_orn = p.getBasePositionAndOrientation(red_ball)
blue_pos, blue_orn = p.getBasePositionAndOrientation(blue_ball)
# Apply force for the first step of the simulation.
if apply_force:
p.applyExternalForce(red_ball, -1,
forceObj=[-40000, 0, 0],
posObj=red_pos,
flags=p.WORLD_FRAME)
p.applyExternalForce(blue_ball, -1,
forceObj=[40000, 0, 0],
posObj=blue_pos,
flags=p.WORLD_FRAME)
apply_force = 0
sync_actor(blue_ball_actor, blue_ball)
sync_actor(red_ball_actor, red_ball)
# Get various collision information using `p.getContactPoints`.
contact = p.getContactPoints(red_ball, blue_ball, -1, -1)
if len(contact) != 0:
tb.message = "Collision!!"
p.stepSimulation()
if cnt == 50:
showm.exit()
def _reset(self):
# reset state
self.steps = 0
self.done = False
# reset pole on cart in starting poses
p.resetBasePositionAndOrientation(self.cart, (0, 0, 0.08),
(0, 0, 0, 1))
p.resetBasePositionAndOrientation(self.pole, (0, 0, 0.35),
(0, 0, 0, 1))
for _ in xrange(100):
p.stepSimulation()
# give a fixed force push in a random direction to get things going...
theta = (np.random.random() * 2 * np.pi) if self.random_theta else 0.0
fx, fy = self.initial_force * np.cos(
theta), self.initial_force * np.sin(theta)
for _ in xrange(self.initial_force_steps):
p.stepSimulation()
p.applyExternalForce(self.cart, -1, (fx, fy, 0), (0, 0, 0),
p.WORLD_FRAME)
if self.delay > 0:
time.sleep(self.delay)
# bootstrap state by running for all repeats
for i in xrange(self.repeats):
self.set_state_element_for_repeat(i)
# reset event log (if applicable) and add entry with only state
if self.event_log:
self.event_log.reset()
self.event_log.add_just_state(self.state)
# return this state
return np.copy(self.state)
def apply_force_to_body(self, uid, force, position):
pybullet.applyExternalForce(objectUniqueId=uid,
linkIndex=-1,
forceObj=list(force),
posObj=list(position),
flags=pybullet.LINK_FRAME,
physicsClientId=self.uid)
def timer_callback(_obj, _event):
cnt = next(counter)
showm.render()
pos, orn = p.getBasePositionAndOrientation(ball)
base_pos, base_orn = p.getBasePositionAndOrientation(base)
base_actor.SetPosition(base_pos[0], base_pos[1], base_pos[2])
if storage.f:
print("entered")
for j in range(5):
p.applyExternalForce(ball,
-1,
forceObj=[-2000, 0, 0],
posObj=pos,
flags=p.WORLD_FRAME)
storage.f = 0
# Updating brick position
for i, brick_row in enumerate(brick_actors):
for j, brick_actor in enumerate(brick_row):
ball_actor.SetPosition(pos[0], pos[1], pos[2])
brick_pos, brick_orn = p.getBasePositionAndOrientation(
brick_Ids[i][j])
brick_actor.SetPosition(brick_pos[0], brick_pos[1], brick_pos[2])
orn_deg = np.degrees(p.getEulerFromQuaternion(brick_orn))
brick_actor.SetOrientation(orn_deg[0], orn_deg[1], orn_deg[2])
brick_actor.RotateWXYZ(orn[1], orn[2], orn[3], orn[0])
p.stepSimulation()
# print(orn)
# storage.orn_prev = orn
if cnt == 2000:
showm.exit()
def set_speeds(self, speeds=[0,0,0,0]): self.speeds = np.array(speeds) forces = np.array(self.speeds**2) * self.attributes["Kf"] torques = np.array(self.speeds**2) * self.attributes["Km"] z_torque = (-torques[0] + torques[1] - torques[2] + torques[3]) for i in range(4): p.applyExternalForce(self.uid, linkIndex=i, forceObj=[0,0,forces[i]], posObj=[0,0,0], flags=p.LINK_FRAME, physicsClientId=self.sim.id) p.applyExternalTorque(self.uid, linkIndex=4, torqueObj=[0,0,z_torque], flags=p.LINK_FRAME, physicsClientId=self.sim.id)
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = -0.5
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = 0.5
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_TRIGGERED)):
forward = 1
if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_RELEASED)):
forward = 0
if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_TRIGGERED)):
forward = -1
if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_RELEASED)):
forward = 0
force = [forward * camForward[0], forward * camForward[1], 0]
cameraYaw = cameraYaw + turn
if (forward):
p.applyExternalForce(sphere, -1, force, spherePos, flags=p.WORLD_FRAME)
p.stepSimulation()
time.sleep(1. / 240.)
