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
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)
time.sleep(dt)
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.)
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)
skip_sync=16
meshcat_utils_dp.sync_visual_transforms(mb, b2vis, vis)
def __init__(self, simulation_time_step):
self.MPC_BODY_MASS = MPC_BODY_MASS
self.MPC_BODY_INERTIA = MPC_BODY_INERTIA
self.MPC_BODY_HEIGHT = MPC_BODY_HEIGHT
self.time_step = simulation_time_step
self.num_legs = NUM_LEGS
self.num_motors = NUM_MOTORS
self.skip_sync = 0
self.world = dp.TinyWorld()
self.world.friction = 10.0
self.mb_solver = dp.TinyMultiBodyConstraintSolver()
self.vis = meshcat.Visualizer(zmq_url='tcp://127.0.0.1:6000')
self.vis.delete()
urdf_parser = dp.TinyUrdfParser()
plane_urdf_data = urdf_parser.load_urdf(
"../../../data/plane_implicit.urdf")
plane2vis = meshcat_utils_dp.convert_visuals(
plane_urdf_data, "../../../data/checker_purple.png", self.vis,
"../../../data/")
self.plane_mb = dp.TinyMultiBody(False)
plane2mb = dp.UrdfToMultiBody2()
res = plane2mb.convert2(plane_urdf_data, self.world, self.plane_mb)
self.urdf_data = urdf_parser.load_urdf(
"../../../data/laikago/laikago_toes_zup_joint_order.urdf")
print("robot_name=", self.urdf_data.robot_name)
self.b2vis = meshcat_utils_dp.convert_visuals(
self.urdf_data, "../../../data/laikago/laikago_tex.jpg", self.vis,
"../../../data/laikago/")
is_floating = True
self.mb = dp.TinyMultiBody(is_floating)
urdf2mb = dp.UrdfToMultiBody2()
self.res = urdf2mb.convert2(self.urdf_data, self.world, self.mb)
self.mb.set_base_position(dp.Vector3(0, 0, 0.6))
knee_angle = -0.5
abduction_angle = 0.2
self.initial_poses = [
abduction_angle, 0., knee_angle, abduction_angle, 0., knee_angle,
abduction_angle, 0., knee_angle, abduction_angle, 0., knee_angle
]
qcopy = self.mb.q
print("mb.q=", self.mb.q)
print("qcopy=", qcopy)
for q_index in range(12):
qcopy[q_index + 7] = self.initial_poses[q_index]
print("qcopy=", qcopy)
self.mb.set_q(qcopy)
print("2 mb.q=", self.mb.q)
print("self.mb.links=", self.mb.links)
print("TDS:")
for i in range(len(self.mb.links)):
print("i=", i)
l = self.mb.links[i]
#print("l.link_name=", l.link_name)
print("l.joint_name=", l.joint_name)
dp.forward_kinematics(self.mb, self.mb.q, self.mb.qd)
self._BuildJointNameToIdDict()
self._BuildUrdfIds()
self._BuildMotorIdList()
self.ResetPose()
self._motor_enabled_list = [True] * self.num_motors
self._step_counter = 0
self._state_action_counter = 0
self._motor_offset = np.array([0.] * 12)
self._motor_direction = np.array([1.] * 12)
self.ReceiveObservation()
self._kp = self.GetMotorPositionGains()
self._kd = self.GetMotorVelocityGains()
self._motor_model = LaikagoMotorModel(
kp=self._kp, kd=self._kd, motor_control_mode=MOTOR_CONTROL_HYBRID)
self.ReceiveObservation()
self.mb.set_base_position(
dp.Vector3(START_POS[0], START_POS[1], START_POS[2]))
self.mb.set_base_orientation(
dp.Quaternion(START_ORN[0], START_ORN[1], START_ORN[2],
START_ORN[3]))
dp.forward_kinematics(self.mb, self.mb.q, self.mb.qd)
meshcat_utils_dp.sync_visual_transforms(self.mb, self.b2vis, self.vis)
self._SettleDownForReset(reset_time=1.0)