def ApplyAction(self, motor_commands, motor_control_mode):
"""Apply the motor commands using the motor model.
Args:
motor_commands: np.array. Can be motor angles, torques, hybrid commands
motor_control_mode: A MotorControlMode enum.
"""
dp.forward_kinematics(self.mb, self.mb.q, self.mb.qd)
dp.forward_dynamics(self.mb, dp.Vector3(0., 0., -10.))
mb_q = vecx_to_np(self.mb.q)[7:]
mb_qd = vecx_to_np(self.mb.qd)[6:]
actual_torque, observed_torque = self._motor_model.convert_to_torque(
motor_commands,
mb_q,
mb_qd,
mb_qd,
motor_control_mode=motor_control_mode)
#print("actual_torque=",actual_torque)
for i in range(len(actual_torque)):
self.mb.tau[i] = actual_torque[i]
collision_shape.geometry.geom_type = pd.SPHERE_TYPE
collision_shape.geometry.sphere.radius = sphere_radius
sphere_mb, sphere2vis = create_multi_body("sphere_link", 2.0, [collision_shape], [visual_shape], True, vis, world)
sphere_mb.set_base_position(pd.TinyVector3(0.,0.,25.))
dt = 1./240.
mb_solver = pd.TinyMultiBodyConstraintSolver()
#texture_path = os.path.join(pd.getDataPath(),
# 'laikago/laikago_tex.jpg')
while 1:
pd.forward_dynamics(sphere_mb, pd.TinyVector3(0.,0.,-10.))
#pd.integrate_q(sphere_mb, dt)
#collision detection
multi_bodies = [plane_mb, sphere_mb]
dispatcher = world.get_collision_dispatcher()
contacts = world.compute_contacts_multi_body(multi_bodies,dispatcher)
#print("contacts=",contacts)
#collision solver
for cps in contacts:
mb_solver.resolve_collision(cps, dt)
pd.integrate_euler(sphere_mb, dt)
meshcat_utils_dp.sync_visual_transforms(sphere_mb, sphere2vis, vis)
qd[0] = -1
qd[1] = 0
qd_new = laikago_mb.qd
#qd_new[2] = 1
#laikago_mb.qd= qd_new
print("laikago_mb.q=", laikago_mb.q)
print("laikago_mb.qd=", laikago_mb.qd)
dt = 1. / 1000.
#p0.setGravity(0,0,-10)
while p0.isConnected():
#dp.forward_kinematics(laikago_mb, laikago_mb.q, laikago_mb.qd)
dp.forward_dynamics(laikago_mb, dp.Vector3(0., 0., -10.))
#dp.forward_dynamics(laikago_mb, dp.Vector3(0., 0., 0.))
if 1:
multi_bodies = [plane_mb, laikago_mb]
contacts = world.compute_contacts_multi_body(multi_bodies, dispatcher)
#collision solver
for cps in contacts:
mb_solver.resolve_collision(cps, dt)
dp.integrate_euler(laikago_mb, dt)
meshcat_utils_dp.sync_visual_transforms(laikago_mb, laikago_vis, vis)
time.sleep(1. / 240.)
print("mb.q=",mb.q)
print("qcopy=",qcopy)
for q_index in range (12):
qcopy[q_index+7]=initial_poses[q_index]
print("qcopy=",qcopy)
mb.set_q(qcopy)
print("2 mb.q=",mb.q)
dt = 1./1000.
skip_sync = 0
frame = 0
while 1:
dp.forward_kinematics(mb, mb.q, mb.qd)
dp.forward_dynamics(mb, dp.Vector3(0.,0.,-10.))
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]