def get_action(self):
"""Computes the torque for stance legs."""
desired_com_position = np.array((0., 0., self._desired_body_height),
dtype=np.float64)
desired_com_velocity = np.array(
(self.desired_speed[0], self.desired_speed[1], 0.),
dtype=np.float64)
desired_com_roll_pitch_yaw = np.array((0., 0., 0.), dtype=np.float64)
desired_com_angular_velocity = np.array(
(0., 0., self.desired_twisting_speed), dtype=np.float64)
foot_contact_state = np.array(
[(leg_state == gait_generator_lib.LegState.STANCE)
for leg_state in self._gait_generator.desired_leg_state],
dtype=np.int32)
# We use the body yaw aligned world frame for MPC computation.
com_roll_pitch_yaw = np.array(self._robot.GetBaseRollPitchYaw(),
dtype=np.float64)
com_roll_pitch_yaw[2] = 0
#predicted_contact_forces=[0]*self._num_legs*_FORCE_DIMENSION
p.submitProfileTiming("predicted_contact_forces")
predicted_contact_forces = self._cpp_mpc.compute_contact_forces(
[0], #com_position
np.asarray(self._state_estimator.com_velocity_body_frame,
dtype=np.float64), #com_velocity
np.array(com_roll_pitch_yaw,
dtype=np.float64), #com_roll_pitch_yaw
# Angular velocity in the yaw aligned world frame is actually different
# from rpy rate. We use it here as a simple approximation.
np.asarray(self._robot.GetBaseRollPitchYawRate(),
dtype=np.float64), #com_angular_velocity
foot_contact_state, #foot_contact_states
self._robot.GetFootPositionsInBaseFrame().flatten(
), #foot_positions_base_frame
self._friction_coeffs, #foot_friction_coeffs
desired_com_position, #desired_com_position
desired_com_velocity, #desired_com_velocity
desired_com_roll_pitch_yaw, #desired_com_roll_pitch_yaw
desired_com_angular_velocity #desired_com_angular_velocity
)
p.submitProfileTiming()
contact_forces = {}
for i in range(self._num_legs):
contact_forces[i] = np.array(
predicted_contact_forces[i * _FORCE_DIMENSION:(i + 1) *
_FORCE_DIMENSION])
action = {}
for leg_id, force in contact_forces.items():
if self._gait_generator.leg_state[
leg_id] == gait_generator_lib.LegState.LOSE_CONTACT:
force = (0, 0, 0)
motor_torques = self._robot.MapContactForceToJointTorques(
leg_id, force)
for joint_id, torque in motor_torques.items():
action[joint_id] = (0, 0, 0, 0, torque)
return action
def get_action(self):
"""Computes the torque for stance legs."""
desired_com_position = np.array((0., 0., self._desired_body_height),
dtype=np.float64)
desired_com_velocity = np.array(
(self.desired_speed[0], self.desired_speed[1], 0.), dtype=np.float64)
desired_com_roll_pitch_yaw = np.array((0., 0., 0.), dtype=np.float64)
desired_com_angular_velocity = np.array(
(0., 0., self.desired_twisting_speed), dtype=np.float64)
foot_contact_state = np.array(
[(leg_state in (gait_generator_lib.LegState.STANCE,
gait_generator_lib.LegState.EARLY_CONTACT))
for leg_state in self._gait_generator.desired_leg_state],
dtype=np.int32)
# We use the body yaw aligned world frame for MPC computation.
com_roll_pitch_yaw = np.array(self._robot.GetBaseRollPitchYaw(),
dtype=np.float64)
com_roll_pitch_yaw[2] = 0
#predicted_contact_forces=[0]*self._num_legs*_FORCE_DIMENSION
# print("Com Vel: {}".format(self._state_estimator.com_velocity_body_frame))
# print("Com RPY: {}".format(self._robot.GetBaseRollPitchYawRate()))
# print("Com RPY Rate: {}".format(self._robot.GetBaseRollPitchYawRate()))
p.submitProfileTiming("predicted_contact_forces")
predicted_contact_forces = self._cpp_mpc.compute_contact_forces(
[0], #com_position
np.asarray(self._state_estimator.com_velocity_body_frame,
dtype=np.float64), #com_velocity
np.array(com_roll_pitch_yaw, dtype=np.float64), #com_roll_pitch_yaw
# Angular velocity in the yaw aligned world frame is actually different
# from rpy rate. We use it here as a simple approximation.
np.asarray(self._robot.GetBaseRollPitchYawRate(),
dtype=np.float64), #com_angular_velocity
foot_contact_state, #foot_contact_states
np.array(self._robot.GetFootPositionsInBaseFrame().flatten(),
dtype=np.float64), #foot_positions_base_frame
self._friction_coeffs, #foot_friction_coeffs
desired_com_position, #desired_com_position
desired_com_velocity, #desired_com_velocity
desired_com_roll_pitch_yaw, #desired_com_roll_pitch_yaw
desired_com_angular_velocity #desired_com_angular_velocity
)
p.submitProfileTiming()
# sol = np.array(predicted_contact_forces).reshape((-1, 12))
# x_dim = np.array([0, 3, 6, 9])
# y_dim = x_dim + 1
# z_dim = y_dim + 1
# print("Y_forces: {}".format(sol[:, y_dim]))
contact_forces = {}
for i in range(self._num_legs):
contact_forces[i] = np.array(
predicted_contact_forces[i * _FORCE_DIMENSION:(i + 1) *
_FORCE_DIMENSION])
action = {}
for leg_id, force in contact_forces.items():
# While "Lose Contact" is useful in simulation, in real environment it's
# susceptible to sensor noise. Disabling for now.
# if self._gait_generator.leg_state[
# leg_id] == gait_generator_lib.LegState.LOSE_CONTACT:
# force = (0, 0, 0)
motor_torques = self._robot.MapContactForceToJointTorques(leg_id, force)
for joint_id, torque in motor_torques.items():
action[joint_id] = (0, 0, 0, 0, torque)
return action, contact_forces
import pybullet as p
import time
#you can visualize the timings using Google Chrome, visit about://tracing
#and load the json file
import pybullet_data
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
t = time.time() + 3.1
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "chrome_about_tracing.json")
while (time.time() < t):
p.stepSimulation()
p.submitProfileTiming("pythontest")
time.sleep(1./240.)
p.submitProfileTiming("nested")
for i in range (100):
p.submitProfileTiming("deep_nested")
p.submitProfileTiming()
time.sleep(1./1000.)
p.submitProfileTiming()
p.submitProfileTiming()
p.stopStateLogging(logId)
import pybullet as p
import time
p.connect(p.GUI)
t = time.time()+0.1
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "haha")
while (time.time()<t):
p.submitProfileTiming("pythontest")
p.stopStateLogging(logId)
import pybullet as p
import time
p.connect(p.GUI)
t = time.time() + 0.1
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "haha")
while (time.time() < t):
p.submitProfileTiming("pythontest")
p.stopStateLogging(logId)
