def step_3d_simulation_shooter(player_body,
bullet_body,
goals,
bodies,
nsteps=1,
physicsClientId=0):
done = False
hit = False
reward = 0
for i in range(nsteps):
p.stepSimulation(physicsClientId=physicsClientId)
bullet_aabb = p.getAABB(bullet_body, physicsClientId=physicsClientId)
obs = p.getOverlappingObjects(*bullet_aabb,
physicsClientId=physicsClientId)
if obs is not None:
obs = [ob[0] for ob in obs]
for ob in obs:
if ob in goals:
x, y = np.random.uniform(0, 10, size=(2))
vx, vy = np.random.uniform(-20.0, 20.0, size=[2])
p.resetBasePositionAndOrientation(
ob, [x, y, 3.0], [0, 0, 0, 1],
physicsClientId=physicsClientId)
p.resetBaseVelocity(ob, [vx, vy, 0],
physicsClientId=physicsClientId)
reward += 1
hit = True
elif ob in bodies:
x, y = np.random.uniform(0, 10, size=(2))
vx, vy = np.random.uniform(-20.0, 20.0, size=[2])
p.resetBasePositionAndOrientation(
ob, [x, y, 3.0], [0, 0, 0, 1],
physicsClientId=physicsClientId)
p.resetBaseVelocity(ob, [vx, vy, 0],
physicsClientId=physicsClientId)
reward -= 1
hit = True
if hit:
p.resetBasePositionAndOrientation(bullet_body, [30, 30, 0.5],
[0, 0, 0, 1],
physicsClientId=physicsClientId)
vel, _ = p.getBaseVelocity(player_body, physicsClientId=physicsClientId)
vel = vel[:2]
tot = sum([abs(v) for v in list(vel)])
if tot > 1:
done = True
else:
done = False
return done, reward
def Reset(self, reload_urdf=True):
"""Reset the minitaur to its initial states.
Args:
reload_urdf: Whether to reload the urdf file. If not, Reset() just place
the minitaur back to its starting position.
"""
if reload_urdf:
if self._self_collision_enabled:
self.quadruped = pybullet.loadURDF(
"quadruped/minitaur.urdf",
INIT_POSITION,
flags=pybullet.URDF_USE_SELF_COLLISION)
else:
self.quadruped = pybullet.loadURDF("quadruped/minitaur.urdf",
INIT_POSITION)
self._BuildJointNameToIdDict()
self._BuildMotorIdList()
self._RecordMassInfoFromURDF()
self.ResetPose(add_constraint=True)
if self._on_rack:
pybullet.createConstraint(self.quadruped, -1, -1, -1,
pybullet.JOINT_FIXED, [0, 0, 0],
[0, 0, 0], [0, 0, 1])
else:
pybullet.resetBasePositionAndOrientation(self.quadruped,
INIT_POSITION,
INIT_ORIENTATION)
pybullet.resetBaseVelocity(self.quadruped, [0, 0, 0], [0, 0, 0])
self.ResetPose(add_constraint=False)
self._overheat_counter = np.zeros(self.num_motors)
self._motor_enabled_list = [True] * self.num_motors
def reset_obj_state(self):
assert self.num_objs == len(self.obj_state)
for obj_handle, (pos, orn, lin_vel, ang_vel) in zip(self.obj_ids, self.obj_state):
p.resetBasePositionAndOrientation(obj_handle, pos, orn)
p.resetBaseVelocity(obj_handle, lin_vel, ang_vel)
for _ in range(100):
self.step_simulation()
def update(self):
import numpy as np
pos = np.array(p.getBasePositionAndOrientation(self._agent)[0])
vel = np.array(p.getBaseVelocity(self._agent)[0])
pos = pos + (vel*self._dt)
# print ("vel: ", vel)
# pos = clampValue(pos, self._pos_bounds) ### Don't do this allow epoch to reset instead.
pos[2] = 0.5
### Need to do this so the intersections are updated
p.stepSimulation()
p.resetBasePositionAndOrientation(self._agent, pos, p.getQuaternionFromEuler([0.,0,0]))
### This must be after setting position, because setting the position removes the velocity
p.resetBaseVelocity(self._agent, linearVelocity=vel, angularVelocity=[0,0,0])
reward = self.computeReward(state=None)
# print("reward: ", reward)
### Change goal location if reached
pos = np.array(p.getBasePositionAndOrientation(self._agent)[0])
posT = np.array(p.getBasePositionAndOrientation(self._target)[0])
goalDirection = posT-pos
goalDistance = np.sqrt((goalDirection*goalDirection).sum(axis=0))
if ((goalDistance < self._reach_goal_threshold)):
x = (np.random.rand()-0.5) * self._map_area * 2.0
y = (np.random.rand()-0.5) * self._map_area * 2.0
# x = (np.random.rand()-0.5) * 2.0 * 2.0
# y = (np.random.rand()-0.5) * 2.0 * 2.0
p.resetBasePositionAndOrientation(self._target, [x,y,0.5], p.getQuaternionFromEuler([0.,0,0]))
p.resetBaseVelocity(self._target, [0,0,0], [0,0,0])
self.__reward = reward
def reset(self, force_randomize=None):
# If we are randomizing env every episode, then delete everything and load robot again
if self.config["randomize_env"]:
self.robot, self.plane = self.load_robot()
# Reset PID variables
self.pid_controller.setup_stabilization_control()
self.step_ctr = 0
self.episode_ctr += 1
self.current_disturbance = None
self.prev_act = None
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"])
]
if self.config["rnd_init"]:
difc = self.config["init_difficulty"]
rnd_starting_pos_delta = np.random.rand(3) * 3. * difc - 1.5 * difc
rnd_starting_orientation = p.getQuaternionFromEuler(
np.random.rand(3) * 2 * difc - 1 * difc,
physicsClientId=self.client_ID)
rnd_starting_lin_velocity = np.random.rand(3) * 2 * difc - 1 * difc
rnd_starting_rot_velocity = np.random.rand(
3) * 1 * difc - .5 * difc
else:
rnd_starting_pos_delta = np.zeros(3)
rnd_starting_orientation = np.array([0, 0, 0, 1])
rnd_starting_lin_velocity = np.zeros(3)
rnd_starting_rot_velocity = np.zeros(3)
p.resetJointState(self.robot,
0,
targetValue=0,
targetVelocity=0,
physicsClientId=self.client_ID)
p.resetBasePositionAndOrientation(self.robot,
self.config["starting_pos"] +
rnd_starting_pos_delta,
rnd_starting_orientation,
physicsClientId=self.client_ID)
p.resetBaseVelocity(self.robot,
linearVelocity=rnd_starting_lin_velocity,
angularVelocity=rnd_starting_rot_velocity,
physicsClientId=self.client_ID)
obs, _, _, _ = self.step(np.zeros(self.act_dim) + 0.1)
return obs
def set_state(self, pos=None, ori=None, vel=None, angvel=None):
# pos
if pos is None:
pos = self.get_pos()
pos = totensor(pos)
# ori
if ori is None:
ori = self.get_ori()
ori = totensor(ori)
if ori.shape == (3, 3):
r = R.from_matrix(ori)
ori = torch.tensor(r.as_quat())
elif ori.shape == (3, ):
r = R.from_euler('xyz', ori, degrees=False)
ori = torch.tensor(r.as_quat())
# vel
if vel is None:
vel = self.get_vel()
# angvel
if angvel is None:
angvel = self.get_angvel()
# reset
p.resetBasePositionAndOrientation(self.uid,
posObj=pos.tolist(),
ornObj=ori.tolist(),
physicsClientId=self.sim.id)
p.resetBaseVelocity(self.uid,
linearVelocity=vel.tolist(),
angularVelocity=angvel.tolist(),
physicsClientId=self.sim.id)
def update(self):
pos = np.array(pybullet.getBasePositionAndOrientation(self._demon)[0])
vel = np.array(pybullet.getBaseVelocity(self._demon)[0])
pos = pos + (vel * self._dt)
pos[2] = 0.5
# Need to do this so the intersections are computed
for i in range(self.sim_steps):
for particle in self._particles:
pos, ori = [
np.array(_)
for _ in pybullet.getBasePositionAndOrientation(particle)
]
lin_vel, ang_vel = pybullet.getBaseVelocity(particle)
pos[-1] = self._wall_heights[0]
pybullet.resetBasePositionAndOrientation(particle,
posObj=pos,
ornObj=ori)
# noise = np.random.normal(loc=0,scale=0.1,size=3)
pybullet.resetBaseVelocity(particle, lin_vel, ang_vel)
pybullet.stepSimulation()
for particle in self._particles:
target_base_vel = pybullet.getBaseVelocity(particle)[0]
updated_vel = target_base_vel + np.random.normal(
loc=0., size=(3, ), scale=0.75)
updated_vel[-1] = 0.
pybullet.resetBaseVelocity(particle, linearVelocity=updated_vel)
reward = self.computeReward(state=None)
self.__reward = reward
def resetBody(self):
if len(self.oldDebugInfo) > 0:
for x in self.oldDebugInfo:
p.removeUserDebugItem(x)
p.resetBasePositionAndOrientation(self.quadruped, self.init_position,
[0, 0, 0, 1])
p.resetBaseVelocity(self.quadruped, [0, 0, 0], [0, 0, 0])
def updateAction(self, action):
import numpy as np
"""
action[0] == hlc action
action[1] == llc action
"""
self._hlc_timestep = self._hlc_timestep + 1
if ("dont_do_hrl_logic" in self._game_settings
and (self._game_settings["dont_do_hrl_logic"] == True)):
self._llc_target = clampValue([action[0][0], action[0][1], 0], self._vel_bounds)
else:
self._llc_target = clampValue([action[0][0], action[0][1], 0], self._vel_bounds)
### apply delta position change.
action_ = np.array([action[1][0], action[1][1], 0])
agentVel = np.array(p.getBaseVelocity(self._agent)[0])
# print ("action_: ", action_, " agentVel: ", agentVel)
action_ = agentVel + action_
action_ = clampValue(action_, self._vel_bounds)
if ("use_hlc_action_directly" in self._game_settings
and (self._game_settings["use_hlc_action_directly"] == True)):
action_ = self._llc_target
# print ("New action: ", action_)
p.resetBaseVelocity(self._agent, linearVelocity=action_, angularVelocity=[0,0,0])
def setStartingPositionAndVelocity(self, inputFrame):
# set the agent's root position and orientation
p.resetBasePositionAndOrientation(
self.humanoidAgent,
posObj=inputFrame[0:3],
ornObj=inputFrame[3:7]
)
# set the agent's root velocity and angular velocity
p.resetBaseVelocity(
self.humanoidAgent,
linearVelocity=inputFrame[7:10],
angularVelocity=inputFrame[10:13]
)
# Set joint positions
revoluteJointIndices = [13, 15, 17, 19]
for i, joint in enumerate(self.revoluteJoints):
p.resetJointState(
self.humanoidAgent,
jointIndex=joint,
targetValue=inputFrame[revoluteJointIndices[i]],
targetVelocity=inputFrame[revoluteJointIndices[i]+1]
)
sphericalJointIndices = [21, 28, 35, 42, 49, 56, 63, 70]
p.resetJointStatesMultiDof(
self.humanoidAgent,
jointIndices=self.sphericalJoints,
targetValues=[inputFrame[index:index+4] for index in sphericalJointIndices],
targetVelocities=[inputFrame[index+4:index+7] for index in sphericalJointIndices]
)
# slight delay is needed before agent will reset. Need to investigate before removing this delay
time.sleep(0.005)
def reset(self):
self._set_cmd = (self.high_bnds + self.low_bnds) / 2.0
self._flip_hold = 0.
# bullet_client.changeDynamics(self.pizza_id, -1, linearDamping=0.1, angularDamping=0.2)
index = 0
for j in range(bullet_client.getNumJoints(self.robot_id)):
# bullet_client.changeDynamics(self.robot_id, j, linearDamping=0, angularDamping=0)
info = bullet_client.getJointInfo(self.robot_id, j)
jointName = info[1]
jointType = info[2]
if (jointType == bullet_client.JOINT_PRISMATIC):
bullet_client.resetJointState(self.robot_id, j,
jointPositions[index])
index = index + 1
if (jointType == bullet_client.JOINT_REVOLUTE):
bullet_client.resetJointState(self.robot_id, j,
jointPositions[index])
index = index + 1
self.__prevPose = bullet_client.getLinkState(self.robot_id,
pandaEndEffectorIndex)
bullet_client.resetBasePositionAndOrientation(self.pizza_id,
init_pizza_pose,
init_pizza_orn)
bullet_client.resetBaseVelocity(self.pizza_id, np.zeros(3),
np.zeros(3))
return self.get_obs()
def reset(self):
p.resetBasePositionAndOrientation(self.walker_id, [0, 0, 0], [0, 0, 0, 1])
for i in range(p.getNumJoints(self.walker_id)):
init_ang = np.random.uniform(low=-self.init_noise, high=self.init_noise)
init_vel = np.random.uniform(low=-self.init_noise, high=self.init_noise)
p.resetJointState(self.walker_id, i, init_ang, init_vel)
init_x = np.random.uniform(low=-self.init_noise, high=self.init_noise)
init_z = np.random.uniform(low=-self.init_noise, high=self.init_noise)
init_pitch = np.random.uniform(low=-self.init_noise, high=self.init_noise)
init_pos = [init_x, 0, init_z]
init_orn = p.getQuaternionFromEuler([0, init_pitch, 0])
p.resetBasePositionAndOrientation(self.walker_id, init_pos, init_orn)
init_vx = np.random.uniform(low=-self.init_noise, high=self.init_noise)
init_vz = np.random.uniform(low=-self.init_noise, high=self.init_noise)
init_vp = np.random.uniform(low=-self.init_noise, high=self.init_noise)
p.resetBaseVelocity(self.walker_id, [init_vx, 0, init_vz], [0, init_vp, 0])
p.setJointMotorControlArray(self.walker_id,
[i for i in range(p.getNumJoints(self.walker_id))],
p.POSITION_CONTROL,
positionGains=[0.1] * self.num_joints,
velocityGains=[0.1] * self.num_joints,
forces=[0 for _ in range(p.getNumJoints(self.walker_id))]
)
return self._get_obs()
def reset(self):
self._done = False
x = (np.random.rand()-0.5) * (self._map_width - 1) * 2.0
y = (np.random.rand()-0.5) * (self._map_width - 1) * 2.0
pybullet.resetBasePositionAndOrientation(self._demon, [x,y,0.5], pybullet.getQuaternionFromEuler([0.,0,0]))
pybullet.resetBaseVelocity(self._demon, [0,0,0], [0,0,0])
x = (np.random.rand()-0.5) * (self._map_width - 1) * 2.0
y = (np.random.rand()-0.5) * (self._map_width - 1) * 2.0
for particle in self._particles:
pybullet.resetBasePositionAndOrientation(particle, [x,y,0.5], pybullet.getQuaternionFromEuler([0.,0,0]))
initial_vel = np.random.normal(loc=0.5, size=(3,), scale=1.)
initial_vel[-1] = 0.
pybullet.resetBaseVelocity(particle, linearVelocity=initial_vel)
# pybullet.resetBaseVelocity(particle, [0,0,0], [0,0,0])
# Reset obstacles
"""
for i in range(len(self._blocks)):
x = (np.random.rand()-0.5) * self._map_width * 2.0
y = (np.random.rand()-0.5) * self._map_width * 2.0
p.resetBasePositionAndOrientation(self._blocks[i], [x,y,0.5], p.getQuaternionFromEuler([0.,0,0]))
p.resetBaseVelocity(self._blocks[i], [0,0,0], [0,0,0])
"""
return self.getObservation()
def reset(self, rpy=None, angvel=None, scope_noise=None):
# scope noise (if specified)
if scope_noise is not None:
self.scope_noise = scope_noise
# reaction wheels
q = np.zeros(self.num_joints)
v = np.zeros(self.num_joints)
for i, joint_id in enumerate(self.joint_ids):
p.resetJointState(self.robot_id, joint_id, q[i], v[i])
# base position, orientation, and velocity
pos = np.array([0., 0., 0.])
if rpy is None:
rpy = 0.1 * self.rng.standard_normal(3)
ori = p.getQuaternionFromEuler(rpy)
p.resetBasePositionAndOrientation(self.robot_id, pos, ori)
if angvel is None:
angvel = 0.1 * self.rng.standard_normal(3)
p.resetBaseVelocity(self.robot_id,
linearVelocity=[0., 0., 0.],
angularVelocity=angvel)
# shooting star position, orientation, and velocity
if self.shootingstar:
self.place_shootingstar()
def reset(self):
self._filt_cmd = 0.
self.t = 0
self._set_cmd = deepcopy(zero_ee_pose)
index = 0
# bullet_client.changeDynamics(self.robot_id, j, linearDamping=0, angularDamping=0.1, restitution=0.)
for j in range(bullet_client.getNumJoints(self.robot_id)):
bullet_client.changeDynamics(self.robot_id,
j,
linearDamping=0,
angularDamping=0.1,
restitution=0.)
info = bullet_client.getJointInfo(self.robot_id, j)
jointName = info[1]
jointType = info[2]
if (jointType == bullet_client.JOINT_PRISMATIC):
bullet_client.resetJointState(self.robot_id, j,
jointPositions[index])
index = index + 1
if (jointType == bullet_client.JOINT_REVOLUTE):
bullet_client.resetJointState(self.robot_id, j,
jointPositions[index])
index = index + 1
self.__prevPose = bullet_client.getLinkState(self.robot_id,
pandaEndEffectorIndex)
bullet_client.resetBasePositionAndOrientation(self.pancake_id,
init_pizza_pose,
init_pancake_rot)
bullet_client.resetBaseVelocity(self.pancake_id, np.zeros(3),
np.zeros(3))
rew, obs = self.get_rew_obs()
return obs
def set_base_velocity(self, linear_velocity, angular_velocity):
"""
Set the linear and angular velocity of the base of a body
"""
p.resetBaseVelocity(
self.id, linear_velocity, angular_velocity, **self._client_kwargs
)
def set_velocities(self, mb_vel_r, joint_vel):
'''
Applies velocities of move base and joints to the simulation.
Args:
mb_vel_r (numpy.array): Base velocities to apply (x, y, theta).
joint_vel (numpy.array): Joint velocities to apply. Length: 7.
'''
# Obtain robot orientation and transform mb vels to world frame
mb_link_state = pb.getLinkState(self.robotId, self.baseLinkId, False,
False, self.clientId)
mb_ang_w = pb.getEulerFromQuaternion(mb_link_state[5])[2]
mb_vel_w = self.rotation_matrix(mb_ang_w).dot(mb_vel_r)
# Apply velocities to simulation
vel_lin = np.append(mb_vel_w[0:2], 0.0)
vel_ang = np.append([0.0, 0.0], mb_vel_w[2])
pb.resetBaseVelocity(self.robotId, vel_lin, vel_ang, self.clientId)
pb.setJointMotorControlArray(self.robotId,
self.joint_mapping,
pb.VELOCITY_CONTROL,
targetVelocities=joint_vel,
forces=len(joint_vel) * [1e10],
physicsClientId=self.clientId)
def resetBaseStates(self,
position=[0, 0, 0],
quaternion=[0, 0, 0, 1],
velocityLinear=[0, 0, 0],
velocityAngular=[0, 0, 0]):
pybullet.resetBasePositionAndOrientation(self.id, position, quaternion)
pybullet.resetBaseVelocity(self.id, velocityLinear, velocityAngular)
def reset_robot():
p.resetBasePositionAndOrientation(
boxId, [0, 0, robot_height], [0, 0, 0, 1])
p.resetBaseVelocity(boxId, [0, 0, 0], [0, 0, 0])
for j in range(12):
p.resetJointState(boxId, motor_id_list[j], init_new_pos[j], init_new_pos[j+12])
cpp_gait_ctrller.init_controller(convert_type(
freq), convert_type([stand_kp, stand_kd, joint_kp, joint_kd]))
for _ in range(10):
p.stepSimulation()
imu_data, leg_data, _ = get_data_from_sim()
cpp_gait_ctrller.pre_work(convert_type(
imu_data), convert_type(leg_data))
for j in range(16):
force = 0
p.setJointMotorControl2(
boxId, j, p.VELOCITY_CONTROL, force=force)
cpp_gait_ctrller.set_robot_mode(convert_type(1))
for _ in range(200):
run()
p.stepSimulation
cpp_gait_ctrller.set_robot_mode(convert_type(0))
def set_position(object_idx, position):
# reuse existing quaternion
_, quaternion = pb.getBasePositionAndOrientation(object_idx)
# resetBasePositionAndOrientation zeroes out velocities, but we wish to conserve them
velocity, angular_velocity = pb.getBaseVelocity(object_idx)
pb.resetBasePositionAndOrientation(object_idx, position, quaternion)
pb.resetBaseVelocity(object_idx, velocity, angular_velocity)
def reset(self):
# Set zero torques
p.setJointMotorControlArray(self.robotId, self.joint_ids, p.TORQUE_CONTROL, forces=[0] * self.act_dim)
p.stepSimulation()
# Reset env variables
self.step_ctr = 0
# Variable positions
obs_dict = {'torso_pos' : (0, 0, 0.7),
'torso_quat' : (0, 0, 0, 1),
'q' : np.zeros(self.n_joints),
'torso_vel' : np.zeros(3),
'torso_angvel' : np.zeros(3),
'q_vel' : np.zeros(self.n_joints)}
obs_arr = np.concatenate([v for v in obs_dict.values()])
# Set environment state
p.resetBasePositionAndOrientation(self.robotId, obs_dict['torso_pos'], obs_dict['torso_quat'])
p.resetBaseVelocity(self.robotId, obs_dict['torso_vel'])
for j in self.joint_ids:
p.resetJointState(self.robotId, j, 0, 0)
p.setJointMotorControlArray(self.robotId, self.joint_ids, p.TORQUE_CONTROL, forces=[0] * self.act_dim)
for i in range(30):
p.stepSimulation()
return obs_arr, obs_dict
def initialise_simulation(self, run_visible=True, randomize_start=True):
self.cur_run_num += 1
self.run_history.append({
'run_num': self.cur_run_num,
'start_params': [],
'end_params': [],
'up_value': 0
})
if run_visible:
connect_type = bt.GUI
else:
connect_type = bt.DIRECT
self.physics_client_id = bt.connect(connect_type)
bt.setPhysicsEngineParameter(fixedTimeStep=self.time_step,
numSolverIterations=100)
# bt.setAdditionalSearchPath(pybullet_data.getDataPath())
bt.setGravity(self.gravity[0], self.gravity[1], self.gravity[2],
self.physics_client_id)
plane_id = bt.createCollisionShape(bt.GEOM_PLANE,
planeNormal=[0, 0, 1])
self.plane_id = bt.createMultiBody(baseMass=0,
baseCollisionShapeIndex=plane_id)
bt.changeDynamics(self.plane_id,
-1,
lateralFriction=0.9,
spinningFriction=1,
restitution=0.1)
# self.plane_id = bt.loadURDF("plane.urdf")
if randomize_start:
# randomize start parameters
self.random_start()
# get obj from die_polygon
exchange_filename = self.transfer_mesh()
# convex mesh from obj
die_collision_shape = bt.createCollisionShape(
bt.GEOM_MESH, fileName=exchange_filename, meshScale=0.01)
os.remove(exchange_filename)
self.die_id = bt.createMultiBody(
baseMass=1,
baseCollisionShapeIndex=die_collision_shape,
basePosition=self.start_pos,
baseOrientation=self.start_rot)
bt.resetBaseVelocity(self.die_id,
linearVelocity=self.start_vel,
angularVelocity=self.start_ang)
bt.changeDynamics(self.die_id,
-1,
lateralFriction=0.9,
spinningFriction=1,
restitution=0.1)
self.run_history[self.cur_run_num]['start_params'] = [
self.start_pos, self.start_vel, self.start_rot, self.start_ang
]
def updateBulletNodes(self, world_name, node_id):
""" This function load the urdf corresponding to the uwds node and set it in the environment
The urdf need to have the same name than the node name
:return:
"""
if self.ctx.worlds()[world_name].scene().root_id() not in self.node_id_map:
self.node_id_map[self.ctx.worlds()[world_name].scene().root_id()] = p.loadURDF("plane.urdf")
node = self.ctx.worlds()[world_name].scene().nodes()[node_id]
if node.type == MESH:
position = [node.position.pose.position.x, node.position.pose.position.y, node.position.pose.position.z]
orientation = [node.position.pose.orientation.x, node.position.pose.orientation.y, node.position.pose.orientation.z, node.position.pose.orientation.w]
if node_id not in self.node_id_map:
try:
self.node_id_map[node_id] = p.loadURDF(node.name+".urdf", position, orientation)
self.reverse_node_id_map[self.node_id_map[node_id]] = node_id
rospy.loginfo("[%s::updateBulletNodes] "+node.name+".urdf' loaded successfully", self.node_name)
except Exception as e:
self.node_id_map[node_id] = -1
if self.node_id_map[node_id] > 0:
self.reverse_node_id_map[self.node_id_map[node_id]] = node_id
else:
if self.node_id_map[node_id] > 0:
p.resetBaseVelocity(self.node_id_map[node_id], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0])
p.resetBasePositionAndOrientation(self.node_id_map[node_id], position, orientation)
else:
self.node_id_map[node_id] = -1
def updateAction(self, action):
import numpy as np
"""
action[0] == hlc action
action[1] == llc action
"""
self._hlc_timestep = self._hlc_timestep + 1
if (self._hlc_timestep >= self._hlc_skip
and (self._ran < 0.5)):
# print ("Updating llc target from HLC")
self._llc_target = clampValue([action[0][0], action[0][1], 0], self._vel_bounds)
### Need to store this target in the sim as a gobal location to allow for computing local distance state.
pos = np.array(p.getBasePositionAndOrientation(self._agent)[0])
# self._llc_target = self._llc_target + action_
self._hlc_timestep = 0
### Update llc action
llc_obs = self.getObservation()[1]
### crazy hack to get proper state size...
# llc_obs = np.concatenate([llc_obs,[0,0,0,0,0,0]])
action[1] = self._llc.predict([llc_obs])
# action[1] = [0.03, -0.023]
# print ("self._llc_target: ", self._llc_target)
### apply delta position change.
action_ = np.array([action[1][0], action[1][1], 0])
agentVel = np.array(p.getBaseVelocity(self._agent)[0])
action_ = agentVel + action_
action_ = clampValue(action_, self._vel_bounds)
if ("use_hlc_action_directly" in self._game_settings
and (self._game_settings["use_hlc_action_directly"] == True)):
action_ = self._llc_target
# print ("New action: ", action)
p.resetBaseVelocity(self._agent, linearVelocity=action_, angularVelocity=[0,0,0])
def reset(self, randomize_vel=True):
p.resetBasePositionAndOrientation(
self.body_id,
posObj=[0, self.y_shift * np.random.choice([-1, 1]), 0],
ornObj=[0, 0, 0, 1])
if randomize_vel:
self.base_vel = self.random_vel()
p.resetBaseVelocity(self.body_id, [self.base_vel, 0, 0])
def set_quaternion(object_idx, quaternion):
quaternion = wxyz2xyzw(quaternion) # convert quaternion format
# reuse existing position
position, _ = pb.getBasePositionAndOrientation(object_idx)
# resetBasePositionAndOrientation zeroes out velocities, but we wish to conserve them
velocity, angular_velocity = pb.getBaseVelocity(object_idx)
pb.resetBasePositionAndOrientation(object_idx, position, quaternion)
pb.resetBaseVelocity(object_idx, velocity, angular_velocity)
def impede(uid):
linVel, angVel = p.getBaseVelocity(uid)
linVel = [i * IMPEDANCE for i in linVel]
angVel = [i * IMPEDANCE for i in angVel]
p.resetBaseVelocity(uid, linVel, angVel)
return linVel, angVel
def OnClickExcute(self, player):
linearVelocity, angularVelocity = p.getBaseVelocity(player.colId)
boostVelocity = vmath.length(vmath.vec3(linearVelocity))
if boostVelocity < 500:
boostVelocity = 500
newVelocity = [0, boostVelocity, 0]
p.resetBaseVelocity(player.colId, newVelocity, angularVelocity)
self.Hide()
def set_body_state(body_id, position, orientation, velocity):
p.resetBasePositionAndOrientation(
body_id,
position,
orientation,
)
linear_vel, angular_vel = velocity
p.resetBaseVelocity(body_id, linear_vel, angular_vel)
def apply_action(self, action):
if self.is_discrete:
realaction = self.action_list[action]
else:
realaction = action
p.setGravity(0, 0, 0)
p.resetBaseVelocity(self.robot_ids[0], realaction[:3], realaction[3:])
def test_rolling_friction(self):
import pybullet as p
p.connect(p.DIRECT)
p.loadURDF("plane.urdf")
sphere = p.loadURDF("sphere2.urdf", [0, 0, 1])
p.resetBaseVelocity(sphere, linearVelocity=[1, 0, 0])
p.changeDynamics(sphere, -1, linearDamping=0, angularDamping=0)
#p.changeDynamics(sphere,-1,rollingFriction=0)
p.setGravity(0, 0, -10)
for i in range(1000):
p.stepSimulation()
vel = p.getBaseVelocity(sphere)
self.assertLess(vel[0][0], 1e-10)
self.assertLess(vel[0][1], 1e-10)
self.assertLess(vel[0][2], 1e-10)
self.assertLess(vel[1][0], 1e-10)
self.assertLess(vel[1][1], 1e-10)
self.assertLess(vel[1][2], 1e-10)
p.disconnect()
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.setPhysicsEngineParameter(enableConeFriction=1)
for i in range (num):
print("progress:",i,num)
x = velocity*math.sin(2.*3.1415*float(i)/num)
y = velocity*math.cos(2.*3.1415*float(i)/num)
print("velocity=",x,y)
sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphere,-1,lateralFriction=0.02)
#p.changeDynamics(sphere,-1,rollingFriction=10)
p.changeDynamics(sphere,-1,linearDamping=0)
p.changeDynamics(sphere,-1,angularDamping=0)
p.resetBaseVelocity(sphere,linearVelocity=[x,y,0])
prevPos = [0,0,0]
for i in range (2048):
p.stepSimulation()
pos = p.getBasePositionAndOrientation(sphere)[0]
if (i&64):
p.addUserDebugLine(prevPos,pos,[1,0,0],1)
prevPos = pos
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
while (1):
time.sleep(0.01)
def reset_velocity(self, linearVelocity=[0,0,0], angularVelocity =[0,0,0]): p.resetBaseVelocity(self.bodies[self.bodyIndex], linearVelocity, angularVelocity)
