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 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 apply_torque_to_body(self, uid, force, position):
pybullet.applyExternalTorque(objectUniqueId=uid,
linkIndex=-1,
forceObj=list(force),
posObj=list(position),
flags=pybullet.LINK_FRAME,
physicsClientId=self.uid)
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)
def moveDrone(self, time_interval, j):
self.detect_obstacle()
self.old_pos, self.old_theta = self.pos, self.theta
# Update self state
self.pos, self.quat = pbl.getBasePositionAndOrientation(self.droneID)
self.theta = pbl.getEulerFromQuaternion(self.quat)
self.state = np.array([i for i in self.pos] + [i for i in self.theta])
self.all_state = np.vstack((self.all_state, self.state))
self.planner.all_distances = np.vstack(
(self.planner.all_distances,
np.array(
[obstacle.getDistance(self) for obstacle in self.obstacles])))
self.thetadot = np.subtract(self.theta, self.old_theta) / time_interval
self.xdot = np.subtract(self.pos, self.old_pos) / time_interval
self.all_xdot.append(self.xdot)
self.pathLength += np.linalg.norm(np.subtract(self.pos, self.old_pos))
start = time()
if not self.targetReached:
if not self.approachingTarget:
self.setDesState()
if abs(np.mean(self.thetadot) <= 0.05) and abs(
np.mean(self.xdot) <= 0.05) and np.linalg.norm(
self.controller.error[0:3]
) <= 0.2 and self.approachingTarget:
self.controller.des_state = [
i for i in self.all_state[-1, 0:2]
] + [0., 0., 0., 0.]
self.targetReached = True
self.times.append(
time() - start
) # At the end we average self.times to get the planner calculation time
self.getInputs()
self.all_input.append(np.array(self.input))
# The following is the actual application of the forces to the drone in simulation to make the drone move
pbl.applyExternalForce(self.droneID, -1,
self.thrust() - self.kd * self.xdot, [0, 0, 0],
pbl.LINK_FRAME)
pbl.applyExternalTorque(self.droneID, -1, self.torque(),
pbl.LINK_FRAME)
self.time += time_interval
# ITSE formula
self.e += time_interval * self.time * (np.linalg.norm(
np.subtract(self.setpoint, self.pos))**2)
def step(self):
self.update_state()
ctrl_pitch = np.clip(
self.pid_pitch_rate.step(self.setpoint_pitch_rate,
self.ang_speed[1], self.cfg['dt']),
*self.cfg['limits']['pid_pitch_rate_out'])
ctrl_roll = np.clip(
self.pid_roll_rate.step(self.setpoint_roll_rate, self.ang_speed[0],
self.cfg['dt']),
*self.cfg['limits']['pid_roll_rate_out'])
ctrl_yaw = np.clip(
self.pid_yaw_rate.step(self.setpoint_yaw_rate, self.ang_speed[2],
self.cfg['dt']),
*self.cfg['limits']['pid_yaw_rate_out'])
translation_forces = np.clip(
np.array([
self.thrust + ctrl_roll - ctrl_pitch,
self.thrust - ctrl_roll - ctrl_pitch,
self.thrust + ctrl_roll + ctrl_pitch,
self.thrust - ctrl_roll + ctrl_pitch,
]) + np.random.uniform(-self.cfg['force_motor_noise'],
self.cfg['force_motor_noise'], 4),
*self.cfg['limits']['motor_force'])
yaw_torque = np.clip(ctrl_yaw, *self.cfg['limits']['yaw_torque'])
motor_points = np.array([[1, 1, 0], [1, -1, 0], [-1, 1, 0],
[-1, -1, 0]]) * 0.2
for force_point, force in zip(motor_points, translation_forces):
p.applyExternalForce(self.body_id,
-1, [0, 0, force],
force_point,
p.LINK_FRAME,
physicsClientId=self.pybullet_client)
p.applyExternalTorque(self.body_id,
0, [0, 0, yaw_torque],
p.LINK_FRAME,
physicsClientId=self.pybullet_client)
# apply noise and wind
p.applyExternalForce(self.body_id,
-1, (np.random.rand(3) - 0.5) *
self.cfg['force_noise_fac'],
self.position,
p.WORLD_FRAME,
physicsClientId=self.pybullet_client)
p.applyExternalTorque(self.body_id,
0, (np.random.rand(3) - 0.5) *
self.cfg['torque_noise_fac'],
p.LINK_FRAME,
physicsClientId=self.pybullet_client)
p.applyExternalForce(self.body_id,
-1,
self.simulated_wind_vector,
self.position,
p.WORLD_FRAME,
physicsClientId=self.pybullet_client)
def update(self, action):
"""Call the pybullet function to apply the desired force to this model.
"""
self.rotor_speed = self.rotor_speed + (
action - self.rotor_speed) * self.spool_up_rate
p.applyExternalForce(self.uid, self.frame_id,
[0, 0, self.max_thrust * self.rotor_speed],
[0, 0, 0], p.LINK_FRAME)
p.applyExternalTorque(self.uid, self.frame_id,
[0, 0, self.max_torque * self.rotor_speed],
p.LINK_FRAME)
def applyExternalTorque(objectUniqueId: int, torqueObj: Vec3, frame: Frame):
assert isinstance(torqueObj, Vec3)
# There is a bug in Pybullet that the Link Frame and World frame are swapped when applying a torque to the
# base link of a robot (https://github.com/bulletphysics/bullet3/issues/1949)
if frame == Frame.WORLD_FRAME:
frame = Frame.LINK_FRAME
else:
frame = Frame.WORLD_FRAME
p.applyExternalTorque(objectUniqueId, -1, torqueObj.as_nwu(), flags=frame.value)
def apply_torque(self, object_name, torque):
""" Applies a 3D torque on a particular object.
:param object_name: Name identifier of the object.
:param force: 3D torque to apply as a KDL Vector.
:return: nothing
"""
object_id = self.get_object_with_name(object_name)
torque_list = kdl_vector_to_list(torque)
link_id = -1
# print object_id, torque_list, link_id
p.applyExternalTorque(object_id, link_id, torque_list, p.LINK_FRAME)
def update_physics(delay, quadcopterId, quadcopter_controller):
while quadcopter_controller.sim:
start = time.perf_counter()
# the controllers are evaluated at a slower rate, only once in the
# control_subsample times the controller is evaluated
if quadcopter_controller.sample == 0:
quadcopter_controller.sample = control_subsample
pos_meas, quaternion_meas = p.getBasePositionAndOrientation(
quadcopterId)
force_act1, force_act2, force_act3, force_act4, moment_yaw = quadcopter_controller.update_control(
pos_meas, quaternion_meas)
print(force_act1, type(force_act1),
"HEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE")
else:
quadcopter_controller.sample -= 1
# apply forces/moments from controls etc:
# (do this each time, because forces and moments are reset to zero after a stepSimulation())
p.applyExternalForce(quadcopterId, -1, force_act1,
[arm_length, 0., 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act2,
[0., arm_length, 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act3,
[-arm_length, 0., 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act4,
[0., -arm_length, 0.], p.LINK_FRAME)
# for the yaw-control:
p.applyExternalTorque(quadcopterId, -1, [0., 0., moment_yaw],
p.LINK_FRAME)
# do simulation with pybullet:
p.stepSimulation()
# delay than repeat
calc_time = time.perf_counter() - start
if (calc_time > 1.2 * delay):
#print("Time to update physics is {} and is more than 20% of the desired update time ({}).".format(calc_time,delay))
pass
else:
# print("calc_time = ",calc_time)
while (time.perf_counter() - start < delay):
time.sleep(delay / 10)
def update_physics(delay, quadcopterId, quadcopter_controller):
while quadcopter_controller.sim:
# start = time.clock()#time.perf_counter();
# the controllers are evaluated at a slower rate, only once in the
# control_subsample times the controller is evaluated
# if quadcopter_controller.sample == 0:
# quadcopter_controller.sample = control_subsample
# pos_meas,quaternion_meas = p.getBasePositionAndOrientation(quadcopterId)
# force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas)
# print force_act1,force_act2,force_act3,force_act4,moment_yaw
# else:
# quadcopter_controller.sample -= 1
pos_meas, quaternion_meas = p.getBasePositionAndOrientation(
quadcopterId)
force_act1, force_act2, force_act3, force_act4, moment_yaw = quadcopter_controller.update_control(
pos_meas, quaternion_meas)
# print "LOOK: ", force_act1,force_act2,force_act3,force_act4,moment_yaw
# apply forces/moments from controls etc:
# (do this each time, because forces and moments are reset to zero after a stepSimulation())
p.applyExternalForce(quadcopterId, -1, force_act1,
[arm_length, 0., 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act2,
[0., arm_length, 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act3,
[-arm_length, 0., 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act4,
[0., -arm_length, 0.], p.LINK_FRAME)
# joint_state = p.getJointState(quadcopterId,0)
# print joint_state
# for the yaw-control:
p.applyExternalTorque(quadcopterId, -1, [0., 0., moment_yaw],
p.LINK_FRAME)
# do simulation with pybullet:
p.stepSimulation()
def step(self, action):
# Forward prop neural network to get GRF, use that to change the gravity
# Shoud really compute after every p.stepSimulation
ankle_position, ankle_angular_velocity, appliedTorque, foot_x, foot_y, foot_z, foot_dx, foot_dy, foot_dz, foot_roll, foot_pitch, foot_yaw = self.getFootState(
)
#depth = plate_bottom_z - bed_z
gamma = np.sqrt(foot_dy**2 + foot_dz**2)
beta = foot_roll
if (self.visualizeTrajectory):
self.plotPosition()
customGRF = False
if (foot_z < self.granularDepth and foot_z > 0.0001):
#customGRF = True
pass
#grf_y, grf_z, torque = self.computeGRF(gamma, beta, foot_z, foot_dx, foot_dy, foot_dz, ankle_angular_velocity)
# Step forward some finite number of seconds or milliseconds
self.controller(action[0])
for i in range(3):
foot_index = 3
# Must call this each time we stepSimulation
if (customGRF):
p.applyExternalForce(self.hopper, foot_index,
[0, grf_y, grf_z], [0.0, 0.0, 0.0],
p.LINK_FRAME)
p.applyExternalTorque(self.hopper, foot_index, [torque, 0, 0],
p.LINK_FRAME)
p.stepSimulation()
self.planarConstraint()
isOver = self.checkForEnd()
return self.computeObservation(), self.computeReward(
isOver), isOver, None
def test_torque_gains(gains, force_gains):
c = CubePD(force_gains=force_gains, torque_gains=gains)
obs = env.reset()
cube = env.unwrapped.platform.cube
p.resetDebugVisualizerCamera(cameraDistance=0.6,
cameraYaw=0,
cameraPitch=-40,
cameraTargetPosition=[0, 0, 0])
# HARD EXAMPLE
init_pos = np.array([0., 0., 0.0325])
init_ori = np.array([0.0, 0.0, -1.5634019394850234])
goal_pos = np.array([-0.04638887, -0.00215872, 0.06769445])
goal_ori = np.array(
[-2.2546160857726445, 0.5656166523145225, 1.9566104949225405])
goal_ori = np.array(p.getQuaternionFromEuler(goal_ori))
robot_pos = obs['robot_position']
obs['object_position'] = init_pos
cube.set_state(init_pos, p.getQuaternionFromEuler(init_ori))
total_error = 0
for _ in range(1000):
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, _, _, _ = env.step(robot_pos)
total_error += c._rotation_error(
goal_ori, obs['object_orientation']).magnitude()
return total_error
def step(self, contestview=False):
"""
does one step in the simulation
"""
# current time
self.t = self.time_step * self.dt
# get position of all drones
all_pos = []
for drone in self.drones:
pos, ori = pybullet.getBasePositionAndOrientation(drone['id'])
all_pos.append(pos)
all_pos = np.array(all_pos)
all_done = True
for index, drone in enumerate(self.drones):
# ignore the drone if it is not still running
if not drone['running']:
continue
# check if the drone has just now finished, and if so ignore it
if self.check_ring(drone):
drone['running'] = False
continue
# the drone is not finished, so the simulation should continue
all_done = False
# get state
pos, rpy, linvel, angvel = self.get_state(drone)
# get measurements
pos_meas, rpy_meas, pos_ring, is_last_ring = self.get_sensor_measurements(
drone)
# get actuator commands
try:
tau_x_des, tau_y_des, tau_z_des, f_z_des = drone[
'controller'].run(pos_meas, rpy_meas, pos_ring,
is_last_ring,
np.delete(all_pos, index, axis=0))
xhat = drone['controller'].xhat.flatten()
tau_x, tau_y, tau_z, f_z = self.set_actuator_commands(
tau_x_des, tau_y_des, tau_z_des, f_z_des, drone)
except Exception as err:
print(
f'\n==========\nerror on run of drone {drone["name"]} (turning it off):\n==========\n{traceback.format_exc()}==========\n'
)
drone['running'] = False
continue
# apply rotor forces
pybullet.applyExternalForce(drone['id'], 0,
np.array([0., 0., drone['u'][3]]),
np.array([0., 0., 0.]),
pybullet.LINK_FRAME)
# apply rotor torques
pybullet.applyExternalTorque(
drone['id'], 0,
np.array([drone['u'][0], drone['u'][1], drone['u'][2]]),
pybullet.LINK_FRAME)
# log data
data = drone['data']
data['t'].append(self.t)
data['pos'].append(pos.tolist())
data['rpy'].append(rpy.tolist())
data['linvel'].append(linvel.tolist())
data['angvel'].append(angvel.tolist())
data['pos_meas'].append(pos_meas.tolist())
data['rpy_meas'].append(rpy_meas.tolist())
data['pos_ring'].append(pos_ring.tolist())
data['is_last_ring'].append(is_last_ring)
data['xhat'].append(xhat.tolist())
data['tau_x'].append(tau_x)
data['tau_y'].append(tau_y)
data['tau_z'].append(tau_z)
data['f_z'].append(f_z)
if hasattr(drone['controller'], 'user_data'):
for key, val in drone['controller'].user_data.items():
if key in data['user_data'].keys():
data['user_data'][key].append(val)
else:
data['user_data'][key] = [val]
# try to stay real-time
if self.display:
t = self.start_time + (self.dt * (self.time_step + 1))
time_to_wait = t - time.time()
while time_to_wait > 0:
time.sleep(0.9 * time_to_wait)
time_to_wait = t - time.time()
# take a simulation step
pybullet.stepSimulation()
# increment time step
self.time_step += 1
# update camera
if contestview:
self.camera_update_contest()
else:
self.camera_update()
return all_done
ftz = get_ftz_from_F_c(F_c)
# dV_s = np.array([[0, 0, 0, 1, 0, 0]]).T
# dw_c = s["R_cs"] @ dV_s[0:3]
# dv_c = s["R_cs"] @ dV_s[3:]
# dV_c = np.concatenate((dw_c, dv_c))
# ftz = get_ftz_from_V_c_dV_c(s["V_c"], dV_c.squeeze())
# Normalize if needed
# ftz = normalize_ftz(ftz)
if damping:
# Apply damping from water
M_c_damping = -1 * s["w_c"]
p.applyExternalTorque(robot_id, -1, M_c_damping.tolist(), flags=p.WORLD_FRAME)
# TODO bug in pybullet.
# TODO WORLD_FRAME and link frame are inverted https://github.com/bulletphysics/bullet3/issues/1949
F_c_damping = -10 * s["v_c"]
p.applyExternalForce(robot_id, -1, F_c_damping.tolist(), [0, 0, 0], flags=p.LINK_FRAME)
# TODO add buoyancy force and torque.
for link_idx, ftz_i in enumerate(np.nditer(ftz)):
# Apply control forces
p.applyExternalForce(robot_id, link_idx, [0, 0, ftz_i], [0, 0, 0], flags=p.LINK_FRAME)
# Visualize
p_zero_t = np.array([0, 0, 0, 1])
p_fz_t = np.array([0, 0, ftz_i, 1])
#### Apply y-axis torque to the base ###############################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### Apply z-axis torque to the base ###############################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### To propeller 0 ################################################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### To the center of mass #########################################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
p.applyExternalTorque(DRONE_IDS[0],
linkIndex=4,
torqueObj=[0., 0., 5e-6],
flags=p.LINK_FRAME,
physicsClientId=PYB_CLIENT)
#### Step, sync the simulation, printout the state #################################################
p.stepSimulation(physicsClientId=PYB_CLIENT)
sync(i, START, env.TIMESTEP)
if i % env.SIM_FREQ == 0: env.render()
#### Reset #########################################################################################
if i > 1 and i % ((ARGS.duration_sec /
(ARGS.num_resets + 1)) * env.SIM_FREQ) == 0:
env.reset()
p.setGravity(0, 0, 0, physicsClientId=PYB_CLIENT)
#### Close the environment #########################################################################
thrust_output = tm.thruster_output(desired_force, desired_torque)
# if debug:
# print(f"Thruster Outputs: {np.around(thrust_output, 4)}")
robot_pos, robot_ori = p.getBasePositionAndOrientation(sim.robot)
t_vel, r_vel = p.getBaseVelocity(sim.robot)
t_vel, r_vel = np.array(t_vel), np.array(r_vel)
t_drag = -100.0 * np.sqrt(t_vel.dot(t_vel)) * t_vel
r_drag = -3.0 * np.array(r_vel)
if debug:
p.addUserDebugLine(robot_pos, robot_pos + t_vel * 2, [1, 0, 0], lifeTime=0, lineWidth=3, replaceItemUniqueId=vel_line)
p.applyExternalTorque(sim.robot, -1, r_drag, flags=p.LINK_FRAME)
p.applyExternalForce(sim.robot, -1, t_drag, robot_pos, flags=p.WORLD_FRAME)
if debug:
p.addUserDebugLine(sim.com_position(), sim.com_position() + np.array([0, 0, 0.2]), [1, 0, 0], lifeTime=0, lineWidth=2, replaceItemUniqueId=com_line)
p.addUserDebugLine(cob_position(), cob_position() + np.array([0, 0, 0.2]), [0, 1, 0], lifeTime=0, lineWidth=2, replaceItemUniqueId=bou_line)
# print(f"Distance Between COM and COB: {sim.com_position() - cob_position()}")
for thrust_num, joint in enumerate(thrusters):
state = p.getLinkState(sim.robot, joint, computeLinkVelocity=0)
joint_pos = np.array(state[0])
thrust_axis = np.matmul(np.array(p.getMatrixFromQuaternion(state[1])).reshape(3, 3), np.array([0, 0, 1]))
thrust_vec = thrust_axis / mag(thrust_axis) * thrust_output[thrust_num]
if mag(thrust_vec) > (3.71 * 1.01):
p.applyExternalForce(body,
i,
force[0], [0, 0, 0],
flags=p.LINK_FRAME)
else:
for i in range(-1, p.getNumJoints(body)):
pose = get_world_inertial_pose(body, i)
p.applyExternalForce(body,
i, [0, 0, 10],
pose[0],
flags=p.WORLD_FRAME)
if test_case == 1:
if apply_torque_local:
p.applyExternalTorque(body,
-1, [0, 0, 100],
flags=p.LINK_FRAME)
else:
p.applyExternalTorque(body,
-1, [0, 0, 100],
flags=p.WORLD_FRAME)
else:
if apply_torque_local:
p.applyExternalTorque(body,
-1, [0, 0, 100],
flags=p.LINK_FRAME)
p.applyExternalTorque(body, 0, [0, 0, 100], flags=p.LINK_FRAME)
else:
p.applyExternalTorque(body,
-1, [0, 0, 100],
flags=p.WORLD_FRAME)
def test_simulator_friction_isometry():
import os
from tampc import cfg
import pybullet_data
init_block_pos = [0.0, 0.0]
init_block_yaw = -0.
physics_client = p.connect(p.GUI)
p.setTimeStep(1. / 240.)
p.setRealTimeSimulation(False)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
blockId = p.loadURDF(os.path.join(cfg.ROOT_DIR, "block_big.urdf"),
tuple(init_block_pos) + (-0.02, ),
p.getQuaternionFromEuler([0, 0, init_block_yaw]))
planeId = p.loadURDF("plane.urdf", [0, 0, -0.05], useFixedBase=True)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(cameraDistance=0.5,
cameraYaw=0,
cameraPitch=-85,
cameraTargetPosition=[0, 0, 1])
p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "test_invariant.mp4")
STATIC_VELOCITY_THRESHOLD = 1e-6
def _observe_block(blockId):
blockPose = p.getBasePositionAndOrientation(blockId)
xb = blockPose[0][0]
yb = blockPose[0][1]
roll, pitch, yaw = p.getEulerFromQuaternion(blockPose[1])
return np.array((xb, yb, yaw))
def _static_environment():
v, va = p.getBaseVelocity(blockId)
if (np.linalg.norm(v) > STATIC_VELOCITY_THRESHOLD) or (
np.linalg.norm(va) > STATIC_VELOCITY_THRESHOLD):
return False
return True
p.setGravity(0, 0, -10)
# p.changeDynamics(blockId, -1, lateralFriction=0.1)
p.changeDynamics(planeId,
-1,
lateralFriction=0.5,
spinningFriction=0.3,
rollingFriction=0.1)
f_mag = 1000
f_dir = np.pi / 4
ft = math.sin(f_dir) * f_mag
fn = math.cos(f_dir) * f_mag
MAX_ALONG = 0.075 + 0.2
for _ in range(100):
p.stepSimulation()
N = 300
yaws = np.zeros(N)
z_os = np.zeros((N, 3))
for simTime in range(N):
# observe difference from pushing
px = _observe_block(blockId)
yaws[simTime] = px[2]
# p.applyExternalForce(blockId, -1, [fn, ft, 0], [-MAX_ALONG, MAX_ALONG, 0.025], p.LINK_FRAME)
p.applyExternalTorque(blockId, -1, [0, 0, 150], p.LINK_FRAME)
p.stepSimulation()
while not _static_environment():
for _ in range(100):
p.stepSimulation()
cx = _observe_block(blockId)
# difference in world frame
dx = get_dx(px, cx)
# difference in block frame
dz = dx_to_dz(px, dx)
z_os[simTime] = dz
logger.info("dx %s dz %s", dx, dz)
time.sleep(0.1)
plot_and_analyze_body_frame_invariance(yaws, z_os)
import os, inspect
currentdir = os.path.dirname(
os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)
import time
import pybullet as p
import pybullet_data
p.connect(p.GUI)
timeStep = 0.01
p.setRealTimeSimulation(0)
p.setTimeStep(timeStep)
p.resetSimulation()
cube0 = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"),
[0, 0, 0])
cube1 = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"),
[2, 0, 0], [0, 0.7071, 0, 0.7071])
cube2 = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"),
[0, 2, 0])
cube3 = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"),
[2, 2, 0], [0, 0.7071, 0, 0.7071])
while 1:
p.stepSimulation()
time.sleep(timeStep)
p.applyExternalTorque(cube0, -1, [1, 0, 0], p.LINK_FRAME)
p.applyExternalTorque(cube1, -1, [1, 0, 0], p.LINK_FRAME)
p.applyExternalTorque(cube2, -1, [1, 0, 0], p.WORLD_FRAME)
p.applyExternalTorque(cube3, -1, [1, 0, 0], p.WORLD_FRAME)
def apply_external_torque_to_base(self, torque, robot_index=1):
p.applyExternalTorque(robot_index,
self.torso_ids[robot_index],
torque,
flags=p.WORLD_FRAME)
p.stepSimulation()
time.sleep(1. / 240.)
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
for joint in range(2, 6):
p.setJointMotorControl2(car,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelS,
force=maxForce)
p.stepSimulation()
time.sleep(1. / 240.)
if (k == ord('r') and (v & p.KEY_IS_DOWN)):
p.applyExternalTorque(car, -1, [0, 0, 180], flags=p.LINK_FRAME)
p.stepSimulation()
time.sleep(1. / 240.)
if (k == ord('r') and (v & p.KEY_WAS_RELEASED)):
p.applyExternalTorque(car, -1, [0, 0, 0], flags=p.LINK_FRAME)
p.stepSimulation()
time.sleep(1. / 240.)
if (k == ord('a') and (v & p.KEY_IS_DOWN)):
targetVel = targetVel + 1
targetVel1 = targetVel1 + 0.5
targetVel2 = targetVel2 - 0.5
targetVelRev = targetVelRev - 1
while (1):
br = 0
def update_physics(self, delay, quadcopterId, quadcopter_controller,
cmd_sub, imu_pub, range_pub):
vol_meas = p.getBaseVelocity(quadcopterId)
one_over_sample_rate = 1 / delay
previous_publish = time.perf_counter()
try:
while not rospy.is_shutdown():
start = time.perf_counter()
# the controllers are evaluated at a slower rate, only once in the
# control_subsample times the controller is evaluated
pos_meas, quaternion_meas = p.getBasePositionAndOrientation(
quadcopterId)
previous_vol_meas = np.array(vol_meas)
vol_meas = np.array(p.getBaseVelocity(quadcopterId))
#print(vol_meas, type(vol_meas))
acc = (vol_meas - previous_vol_meas) * one_over_sample_rate
#print("VOL MEAS", previous_vol_meas, acc)
if quadcopter_controller.sample == 0:
quadcopter_controller.sample = control_subsample
#force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas)
actual_values = cmd_sub.get_control()
vectorizer = np.zeros([5, 3])
vectorizer[:, 2] = actual_values
#print("input forces ", actual_values)
force_act1, force_act2, force_act3, force_act4, moment_yaw = vectorizer
#print("Well well well\t\t", vectorizer, force_act1, moment_yaw)
else:
quadcopter_controller.sample -= 1
# apply forces/moments from controls etc:
# (do this each time, because forces and moments are reset to zero after a stepSimulation())
#assuming right is -y
p.applyExternalForce(quadcopterId, -1, force_act1,
[arm_length, -arm_length, 0.],
p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act2,
[-arm_length, -arm_length, 0.],
p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act3,
[-arm_length, arm_length, 0.],
p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act4,
[arm_length, arm_length, 0.],
p.LINK_FRAME)
# for the yaw-control:
p.applyExternalTorque(quadcopterId, -1, moment_yaw,
p.LINK_FRAME)
# do simulation with pybullet:
p.stepSimulation()
header = Header()
header.frame_id = 'Body'
header.stamp = rospy.Time.now()
Tsample_physics
imu_message = Imu()
imu_message.header = header
imu_message.orientation.x = quaternion_meas[0]
imu_message.orientation.y = quaternion_meas[1]
imu_message.orientation.z = quaternion_meas[2]
imu_message.orientation.w = quaternion_meas[3]
imu_message.linear_acceleration.x = acc[0, 0]
imu_message.linear_acceleration.y = acc[0, 1]
imu_message.linear_acceleration.z = acc[0, 2]
msg = Range()
msg.header = header
msg.max_range = 200
msg.min_range = 0
msg.range = pos_meas[2]
if start - previous_publish > 1 / 100:
imu_pub.publish(imu_message)
range_pub.publish(msg)
previous_publish = start
#print("BUBLISHED RANON /simulation/infrared_sensor_node", msg.range)
# delay than repeat
calc_time = time.perf_counter() - start
if (calc_time > delay):
#print("Time to update physics is {} and is more than 20% of the desired update time ({}).".format(calc_time,delay))
pass
else:
# print("calc_time = ",calc_time)
while (time.perf_counter() - start < delay):
time.sleep(delay / 10)
except KeyboardInterrupt:
self.ctrl_c_reset()
def _step(self, action):
p.stepSimulation()
# This is to controll manually the quad.
if self._renders:
time.sleep(self.timeStep)
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
self.speedX = 0.01
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
self.speedX = 0
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
self.speedX = -0.01
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
self.speedX = 0
if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_TRIGGERED)):
self.forward = 0.05
if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_RELEASED)):
self.forward = 0.0
if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_TRIGGERED)):
self.forward = -0.05
if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_RELEASED)):
self.forward = 0.0
self.dx = self.dx + self.speedX
#self.offset_command = self.offset_command + self.forward
print(self.dx)
#print(keys)
if self._renders:
p.resetBasePositionAndOrientation(self.desired_pos_sphere,
[self.dx, 0, 0], [0, 0, 0, 1])
#if self._renders:
#print(math.fabs(x)/2.4)
#print(math.fabs(self.acceleration)*1)
world_pos, world_ori = p.getBasePositionAndOrientation(self.quad)
world_pos_offset = tuple([world_pos[0] - self.dx]) + world_pos[1:3]
world_vel, world_rot_vel = p.getBaseVelocity(self.quad)
# world_rot_vel = (1, 0, 0)
# To obtain local rot_vel
# Expressing world rotation in quadcopter frame coordinate, required for PD controller.
local_rot_vel = qt.qv_mult(qt.q_conjugate(world_ori), world_rot_vel)
desired_rot_vel = np.array(action[1:4]) * 5
rot_error = np.asarray(local_rot_vel) - desired_rot_vel
self.lpf_rot_error = rot_error * 0.05 + self.lpf_rot_error * 0.95
thrust = (action[0] + 1) * self.zero_thrust
torque = -rot_error * 0.1
ang_power = np.sum(np.abs(np.multiply(torque, local_rot_vel)))
thrust_change = math.fabs((self.last_thrust - thrust))
p.applyExternalTorque(self.quad, -1, -rot_error * 0.1, p.WORLD_FRAME)
p.applyExternalForce(self.quad, -1, [0, 0, thrust], [0, 0, 0],
p.LINK_FRAME)
contacts = p.getContactPoints()
self.state = world_pos_offset + world_ori + world_vel + world_rot_vel
# print(self.state)
touched_ground = 0
if contacts != ():
touched_ground = -100
distance_from_center = np.linalg.norm(
np.asarray(world_pos) - np.asarray([0, 0, 0.5]))
reward = 1 - distance_from_center * 0.5 - ang_power * 0.14 - thrust_change * 0.05 + touched_ground
# print(rot_error)
done = 0
if contacts != ():
done = 1
if distance_from_center > 1:
done = 1
return np.array(self.state), reward, done, {}
def check_stability(verts, faces, gui=False):
# First, check if the file is even rooted.
# If it's not rooted, it can't be stable
if not check_rooted(verts, faces):
return False
# Start up the simulation
mode = p.GUI if gui else p.DIRECT
physicsClient = p.connect(mode)
p.setGravity(0, 0, -9.8)
# Load the ground plane
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# print(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
# Convert the object to a URDF assembly, load it up
# There may be more than one URDF, if the object had more than one connected component
obj2urdf(verts, faces, 'tmp')
objIds = []
startPositions = {}
volumes = {}
heights = {}
bases = {}
for urdf in [f for f in os.listdir('tmp') if os.path.splitext(f)[1] == '.urdf']:
with open(f'tmp/{os.path.splitext(urdf)[0]}.json', 'r') as f:
data = json.loads(f.read())
startPos = data['start_pos']
startOrientation = p.getQuaternionFromEuler([0,0,0])
objId = p.loadURDF(f"tmp/{urdf}",startPos, startOrientation)
objIds.append(objId)
startPositions[objId] = startPos
volumes[objId] = data['volume']
heights[objId] = data['height']
bases[objId] = data['base']
shutil.rmtree('tmp')
# Disable collisions between all objects (we only want collisions between objects and the ground)
# That's because we want to check if the different components are *independently* stable, and
# having them hit each other might muck up that judgment
for i in range(0, len(objIds)-1):
ni = p.getNumJoints(objIds[i])
for j in range(i+1, len(objIds)):
nj = p.getNumJoints(objIds[j])
for k in range(-1, ni):
for l in range(-1, nj):
p.setCollisionFilterPair(objIds[i], objIds[j], k, l, False)
import math
# See if objects are stable under some perturbation
for objId in objIds:
s = volumes[objId]
b = bases[objId]
v = s * b
# 800, 4, 4, 4
p.applyExternalForce(objId, -1, (0, 0, 1000*v), startPositions[objId], p.WORLD_FRAME)
p.applyExternalTorque(objId, -1, (0, 4*v, 0), p.WORLD_FRAME)
p.applyExternalTorque(objId, -1, (4*v, 0, 0), p.WORLD_FRAME)
p.applyExternalTorque(objId, -1, (0, 0, 80*v), p.WORLD_FRAME)
# Run simulation
if gui:
for i in range(600):
p.stepSimulation()
time.sleep(1./600.)
else:
for i in range(10000):
p.stepSimulation()
for objId in objIds:
endPos, _ = p.getBasePositionAndOrientation(objId)
zend = endPos[2]
zstart = startPositions[objId][2]
zdiff = abs(zstart - zend)
if zdiff > abs(0.025 * heights[objId]):
p.disconnect()
return False
p.disconnect()
return True
def step(self, ctrl_raw):
# Add new action to queue
self.act_queue.append(ctrl_raw)
self.act_queue.pop(0)
# Simulate the delayed action
if self.randomized_params["output_transport_delay"] > 0:
ctrl_raw_unqueued = self.act_queue[-self.config["act_input"]:]
ctrl_delayed = self.act_queue[
-1 - self.randomized_params["output_transport_delay"]]
else:
ctrl_raw_unqueued = self.act_queue
ctrl_delayed = self.act_queue[-1]
# Scale the action appropriately (neural network gives [-1,1], pid controller [0,1])
if self.config["controller_source"] == "nn":
ctrl_processed = np.clip(ctrl_delayed, -1, 1) * 0.5 + 0.5
else:
ctrl_processed = ctrl_delayed
# Take into account motor delay
self.update_motor_vel(ctrl_processed)
# Apply motor forces
for i in range(4):
motor_force_w_noise = np.clip(
self.current_motor_velocity_vec[i] *
self.randomized_params["motor_power_variance_vector"][i], 0, 1)
motor_force_scaled = motor_force_w_noise * self.randomized_params[
"motor_force_multiplier"]
p.applyExternalForce(self.robot,
linkIndex=i * 2 + 1,
forceObj=[0, 0, motor_force_scaled],
posObj=[0, 0, 0],
flags=p.LINK_FRAME,
physicsClientId=self.client_ID)
p.applyExternalTorque(
self.robot,
linkIndex=i * 2 + 1,
torqueObj=[
0, 0, self.current_motor_velocity_vec[i] *
self.reactive_torque_dir_vec[i] *
self.config["propeller_parasitic_torque_coeff"]
],
flags=p.LINK_FRAME,
physicsClientId=self.client_ID)
self.apply_external_disturbances()
p.stepSimulation(physicsClientId=self.client_ID)
if self.config["animate"]:
time.sleep(self.config["sim_timestep"])
self.step_ctr += 1
torso_pos, torso_quat, torso_euler, torso_vel, torso_angular_vel = self.get_obs(
)
roll, pitch, yaw = torso_euler
pos_delta = np.array(torso_pos) - np.array(self.config["target_pos"])
crashed = (abs(pos_delta) > 6.0).any() or (
(torso_pos[2] < 0.3) and (abs(roll) > 2.5 or abs(pitch) > 2.5))
if self.prev_act is not None:
action_penalty = np.mean(
np.square(np.array(ctrl_processed) - np.array(self.prev_act))
) * self.config["pen_act_coeff"]
else:
action_penalty = 0
# Calculate true reward
pen_position = np.mean(np.square(pos_delta)) * (
self.config["pen_position_coeff"] +
0 * self.step_ctr / float(self.config["max_steps"]))
pen_rpy = np.mean(np.square(
np.array(torso_euler))) * self.config["pen_rpy_coeff"]
pen_vel = np.mean(np.square(torso_vel)) * self.config["pen_vel_coeff"]
pen_rotvel = np.mean(
np.square(torso_angular_vel)) * self.config["pen_ang_vel_coeff"]
r_true = -action_penalty - pen_position - pen_rpy - pen_rotvel - pen_vel
#print(action_penalty, pen_position, pen_rpy, pen_rotvel, pen_vel)
# Calculate proxy reward (for learning purposes)
pen_position_proxy = np.mean(
np.square(pos_delta)) * self.config["pen_position_coeff"]
pen_yaw_proxy = np.mean(np.square(yaw)) * self.config["pen_yaw_coeff"]
pen_vel_proxy = np.mean(np.square(torso_vel)) * np.maximum(
1. - 3 * pen_position_proxy, 0) * self.config["pen_vel_coeff"]
r = -pen_position_proxy - pen_yaw_proxy - action_penalty - pen_vel_proxy
r = np.clip(r_true, -2, 2)
if self.step_ctr == 1:
r = 0
self.rew_queue.append([r])
self.rew_queue.pop(0)
if self.randomized_params["input_transport_delay"] > 0:
r_unqueued = self.rew_queue[-self.config["rew_input"]:]
else:
r_unqueued = self.rew_queue
done = (self.step_ctr > self.config["max_steps"])
compiled_obs = pos_delta, torso_quat, torso_vel, torso_angular_vel
compiled_obs_flat = [
item for sublist in compiled_obs for item in sublist
]
self.obs_queue.append(compiled_obs_flat)
self.obs_queue.pop(0)
if self.randomized_params["input_transport_delay"] > 0:
obs_raw_unqueued = self.obs_queue[-self.config["obs_input"]:]
else:
obs_raw_unqueued = self.obs_queue
aux_obs = []
for i in range(len(obs_raw_unqueued)):
t_obs = []
t_obs.extend(obs_raw_unqueued[i])
if self.config["act_input"] > 0:
t_obs.extend(ctrl_raw_unqueued[i])
if self.config["rew_input"] > 0:
t_obs.extend(r_unqueued[i])
if self.config["step_counter"] > 0:
def step_ctr_to_obs(step_ctr):
return (float(step_ctr) / self.config["max_steps"]) * 2 - 1
t_obs.extend(
[step_ctr_to_obs(np.maximum(self.step_ctr - i - 1, 0))])
aux_obs.extend(t_obs)
if self.config["latent_input"]:
aux_obs.extend(self.randomized_params_list_norm)
obs = np.array(aux_obs).astype(np.float32)
obs_dict = {
"pos_delta": pos_delta,
"torso_quat": torso_quat,
"torso_vel": torso_vel,
"torso_angular_vel": torso_angular_vel,
"reward": r_true,
"action": ctrl_processed
}
self.prev_act = ctrl_processed
return obs, r, done, {
"obs_dict": obs_dict,
"randomized_params": self.randomized_params,
"true_rew": r_true
}
p.changeDynamics(boxId3, -1, lateralFriction=0.0, restitution=1)
p.createConstraint(cube3, -1, boxId3, -1, p.JOINT_POINT2POINT, [1, 0, 0],
[0, 0, 0], [0, 0, 1])
p.createConstraint(cube4, -1, boxId3, -1, p.JOINT_POINT2POINT, [1, 0, 0],
[0, 0, 0], [0, 0, 1])
sphereStartPos = [0.35, 0, 1]
sphereStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
boxId4 = p.loadURDF("/sphere.urdf", sphereStartPos, sphereStartOrientation)
p.changeDynamics(boxId4, -1, lateralFriction=0.0, restitution=1)
p.createConstraint(cube4, -1, boxId4, -1, p.JOINT_POINT2POINT, [1, 0, 0],
[0, 0, 0], [0, 0, 1])
p.createConstraint(cube5, -1, boxId4, -1, p.JOINT_POINT2POINT, [1, 0, 0],
[0, 0, 0], [0, 0, 1])
sphereStartPos = [0.45, 0, 1]
sphereStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
boxId5 = p.loadURDF("/sphere.urdf", sphereStartPos, sphereStartOrientation)
p.changeDynamics(boxId5, -1, lateralFriction=0.0, restitution=1)
p.createConstraint(cube5, -1, boxId5, -1, p.JOINT_POINT2POINT, [1, 0, 0],
[0, 0, 0], [0, 0, 1])
p.createConstraint(cube6, -1, boxId5, -1, p.JOINT_POINT2POINT, [1, 0, 0],
[0, 0, 0], [0, 0, 1])
i = 0
while True:
if i < 150:
p.applyExternalTorque(boxId1, -1, [0, 100, 0], flags=1)
p.stepSimulation()
i += 1
time.sleep(0.01)
# cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
# print(cubePos,cubeOrn)
p.disconnect()
def update_physics(delay, quadcopterId, quadcopter_controller, config,
graph: datalogger):
while quadcopter_controller.sim:
#start = time.perf_counter()
#start = time.clock()
# the controllers are evaluated at a slower rate, only once in the
# control_subsample times the controller is evaluated
# if quadcopter_controller.sample == 0:
# quadcopter_controller.sample = control_subsample
# pos_meas,quaternion_meas = p.getBasePositionAndOrientation(quadcopterId)
# force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas)
# else:
# quadcopter_controller.sample -= 1
pos_meas, quaternion_meas = p.getBasePositionAndOrientation(
quadcopterId)
rotmat = quaternion2rotation_matrix(quaternion_meas)
front = np.array([pos_meas[0] + 0.1750, pos_meas[1], pos_meas[2]])
back = np.array([pos_meas[0] - 0.1750, pos_meas[1], pos_meas[2]])
left = np.array([pos_meas[0], pos_meas[1] + 0.1750, pos_meas[2]])
right = np.array([pos_meas[0], pos_meas[1] - 0.1750, pos_meas[2]])
front_meas = np.dot(rotmat.T, front.reshape(3, 1)).reshape(3)
back_meas = np.dot(rotmat.T, back.reshape(3, 1)).reshape(3)
left_meas = np.dot(rotmat.T, left.reshape(3, 1)).reshape(3)
right_meas = np.dot(rotmat.T, right.reshape(3, 1)).reshape(3)
force_act1, force_act2, force_act3, force_act4, moment_yaw = quadcopter_controller.update_control(
pos_meas, quaternion_meas, config)
radius = 0.25
ige_force1 = ige_force2 = ige_force3 = ige_force4 = 1.0
if (front_meas[2] < 5 * radius):
ige_force1 = groundEffect.groundEffect(front_meas[2], radius)
if (back_meas[2] < 5 * radius):
ige_force3 = groundEffect.groundEffect(back_meas[2], radius)
if (left_meas[2] < 5 * radius):
ige_force2 = groundEffect.groundEffect(left_meas[2], radius)
if (back_meas[2] < 5 * radius):
ige_force4 = groundEffect.groundEffect(right_meas[2], radius)
if graph.capture:
graph.addpoint(pos_meas[2], force_act1[2])
force_act1 = force_act1 / ige_force1
force_act2 = force_act2 / ige_force2
force_act3 = force_act3 / ige_force3
force_act4 = force_act4 / ige_force4
#capturing forces with altitude to plot
# apply forces/moments from controls etc:
# (do this each time, because forces and moments are reset to zero after a stepSimulation())
p.applyExternalForce(quadcopterId, -1, force_act1,
[config.arm_length, 0., 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act2,
[0., config.arm_length, 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act3,
[-config.arm_length, 0., 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act4,
[0., -config.arm_length, 0.], p.LINK_FRAME)
# for the yaw-control:
p.applyExternalTorque(quadcopterId, -1, [0., 0., moment_yaw],
p.LINK_FRAME)
# do simulation with pybullet:
p.stepSimulation()
def _move_PD(self,
pose_handle_base_world_des,
q_offset,
mech,
last_traj_p,
debug=False,
stable_timeout=100,
unstable_timeout=1000):
finished = False
handle_base_ps = []
for i in itertools.count():
handle_base_ps.append(mech.get_pose_handle_base_world().p)
if self.use_gripper:
self._control_fingers('close', debug=debug)
if (not last_traj_p) and self._at_des_handle_base_pose(
pose_handle_base_world_des, q_offset, mech, 0.01):
return handle_base_ps, False
elif last_traj_p and self._at_des_handle_base_pose(
pose_handle_base_world_des, q_offset, mech,
0.000001) and self._stable(handle_base_ps):
return handle_base_ps, True
elif self._stable(handle_base_ps) and (i > stable_timeout):
return handle_base_ps, True
elif i > unstable_timeout:
return handle_base_ps, True
# get position error of the handle base
p_handle_base_world_err, e_handle_base_world_err = self._get_pose_handle_base_world_error(
pose_handle_base_world_des, q_offset, mech)
# use handle vel or gripper vel to calc velocity error
if not self.use_gripper:
lin_v_com_world_err = p.getLinkState(mech._get_bb_id(), \
mech._get_handle_id(),
computeLinkVelocity=1)[6]
else:
v_tip_world_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
lin_v_com_world_err, omega_com_world_err = self._get_v_com_world_error(
v_tip_world_des)
f = np.multiply(self.k[0], p_handle_base_world_err) + \
np.multiply(self.d[0], lin_v_com_world_err)
# only apply torques if using gripper
if self.use_gripper:
tau = np.multiply(
self.k[1], e_handle_base_world_err
) #+ np.multiply(self.d[1], omega_com_world_err)
else:
tau = [0, 0, 0]
self.errors += [(p_handle_base_world_err, lin_v_com_world_err)]
self.forces += [(f, tau)]
if not self.use_gripper:
bb_id = mech._get_bb_id()
handle_id = mech._get_handle_id()
handle_pos = mech.get_pose_handle_base_world().p
handle_q = mech.get_pose_handle_base_world().q
# transform the force into the LINK_FRAME to apply
f = util.transformation(f, [0., 0., 0.],
handle_q,
inverse=True)
p.applyExternalForce(bb_id, handle_id, f, [0., 0., 0.],
p.LINK_FRAME)
if debug:
p.addUserDebugLine(handle_pos,
np.add(handle_pos,
2 * (f / np.linalg.norm(f))),
lifeTime=.05)
else:
p_com_world, q_com_world = self._get_pose_com_('world')
p.applyExternalForce(self.id, -1, f, p_com_world,
p.WORLD_FRAME)
# there is a bug in pyBullet. the link frame and world frame are inverted
# this should be executed in the WORLD_FRAME
p.applyExternalTorque(self.id, -1, tau, p.LINK_FRAME)
p.stepSimulation()
