def _StepInternal(self, action, motor_control_mode):
self.ApplyAction(action, motor_control_mode)
dp.integrate_euler_qdd(self.mb, self.time_step)
multi_bodies = [self.plane_mb, self.mb]
dispatcher = self.world.get_collision_dispatcher()
contacts = self.world.compute_contacts_multi_body(
multi_bodies, dispatcher)
sync_to_pybullet = False
#collision solver
for cps in contacts:
self.mb_solver.resolve_collision(cps, self.time_step)
dp.integrate_euler(self.mb, self.time_step)
joint_states = []
for q_index in range(12):
#qcopy[q_index+7]=self.initial_poses[q_index]
joint_position = self.mb.q[q_index + 7]
joint_velocity = self.mb.qd[q_index + 6]
if sync_to_pybullet:
self.pybullet_client.resetJointState(
self.quadruped, self._motor_id_list[q_index],
joint_position, joint_velocity)
ps = self.mb.get_base_position()
rn = self.mb.get_base_orientation()
angvel = [self.mb.qd[0], self.mb.qd[1], self.mb.qd[2]]
linvel = [self.mb.qd[3], self.mb.qd[4], self.mb.qd[5]]
pos = [ps[0], ps[1], ps[2]]
orn = [rn.x, rn.y, rn.z, rn.w]
if sync_to_pybullet:
self.pybullet_client.resetBasePositionAndOrientation(
self.quadruped, pos, orn)
self.pybullet_client.resetBaseVelocity(self.quadruped, linvel,
angvel)
self.ReceiveObservation()
self.skip_sync -= 1
if self.skip_sync <= 0:
#print("frame=",frame)
self.skip_sync = 16
meshcat_utils_dp.sync_visual_transforms(self.mb, self.b2vis,
self.vis)
self._state_action_counter += 1
mb_q = vecx_to_np(mb.q)[7:]
mb_qd = vecx_to_np(mb.qd)[6:]
actual_torque, observed_torque = motor_model.convert_to_torque(
initial_poses,
mb_q,
mb_qd,
mb_qd,
motor_control_mode=MOTOR_CONTROL_POSITION)
#print("actual_torque=",actual_torque)
for i in range(len(actual_torque)):
mb.tau[i] = actual_torque[i]
dp.integrate_euler_qdd(mb, dt)
multi_bodies = [plane_mb, mb]
dispatcher = world.get_collision_dispatcher()
contacts = world.compute_contacts_multi_body(multi_bodies,dispatcher)
#collision solver
for cps in contacts:
mb_solver.resolve_collision(cps, dt)
dp.integrate_euler(mb, dt)
frame+=1
skip_sync-=1
if skip_sync<=0:
#print("frame=",frame)