def main():
# one viewer opened through __init__()
robot = MjWam4()
robot.reset(pos=np.ones(4))
robot.step(n_steps=1000)
robot.close()
time.sleep(1.0)
# one viewer opened though start_rendering()
robot = MjWam7(render=False)
robot.reset(pos=np.ones(7))
robot.step(n_steps=1000)
robot.start_rendering()
robot.step(n_steps=1000)
robot.stop_rendering()
robot.step(n_steps=1000)
robot.close()
# two viewers for two robots simultaneously
robot1 = MjWam4()
robot1.reset(pos=np.ones(4))
robot2 = MjWam7()
robot2.reset(pos=np.ones(7))
for _ in range(1000):
robot1.step()
robot2.step()
robot1.close()
robot2.close()
def main():
chain = Wam4IK(base_translation=[0, 0, 0.84],
base_orientation=[0, 0, np.pi / 2])
links = [
'wam/links/base', 'wam/links/shoulder_yaw', 'wam/links/shoulder_pitch',
'wam/links/upper_arm', 'wam/links/forearm',
'wam/links/tool_base_wo_plate', 'wam/links/tool_base_w_plate'
]
x0 = np.array([0, 0, 0.84])
q_test = [[0, 0, 0, 0], [1, 1, 1, 1]]
robot = MjWam4(render=True, g_comp=True)
for q in q_test:
print(f'\n\ntesting for q={q}')
robot.reset(pos=q)
cart = chain.forward_kinematics([0, 0] + q + [0, 0, 0],
full_kinematics=True)
for i in range(7):
print(f'\n{links[i][10:]}')
mj_pos = robot.get_body_full_mat(links[i])[:3, 3] - x0
ikpy_pos = cart[i + 1][:3, 3] - x0
print(f'mj: {mj_pos}')
print(f'ikpy: {ikpy_pos}')
print(f'diff: {mj_pos-ikpy_pos}')
plot_joints(chain, q_test)
# inverse kinematics
x_des = [0.15, 0.86, 1.45]
q = chain.active_from_full(chain.inverse_kinematics(x_des))
robot.set_mocap_pos('endeff_des', x_des)
robot.step(des_pos=q, n_steps=5000)
def main():
robot = MjWam4(xml_path="test.xml")
pos, vel, quat, vel_euler = robot.get_object_state("ball1", True, True,
True, True)
print(pos, vel, quat, vel_euler, sep='\n')
robot.set_object_state("ball1", pos, vel, quat, vel_euler)
def main():
robot = MjWam4(render=True, g_comp=True)
robot.reset(pos=np.ones(4))
robot.step(n_steps=1000)
robot.close()
robot = MjWam7(render=True, g_comp=True)
robot.reset(pos=np.ones(7))
robot.step(n_steps=1000)
def main():
def control_callback(robot):
t = robot.time
q0 = np.zeros(robot.n_dof)
qT = np.ones(robot.n_dof)
T = 3.
robot.pos_des = np.minimum(1., (t / T)) * (qT - q0) + q0
robot.vel_des = np.zeros(robot.n_dof)
robot.tau_des = np.zeros(robot.n_dof)
robot = MjWam4()
robot.set_joint_state(np.zeros(robot.n_dof), np.zeros(robot.n_dof))
robot.set_control_cb(control_callback)
robot.start_spinning()
time.sleep(1.5)
robot.stop_spinning()
robot.stop_rendering()
robot.start_rendering()
robot.start_spinning()
time.sleep(1.5)
robot.stop_spinning()
# when using a new robot instance make sure to hand over the correct
# one to the control callback!
robot.close()
robot = MjWam7()
robot.set_joint_state(np.zeros(robot.n_dof), np.zeros(robot.n_dof))
robot.set_control_cb(control_callback)
robot.start_spinning()
time.sleep(3.0)
robot.stop_spinning()
robot.close()
import numpy as np
from mujoco_robots.robots import MjWam7, MjWam4
if __name__ == "__main__":
# robot = MjWAM7(render=True)
robot = MjWam4(render=True)
goal_state = np.ones(robot.n_dof)
def compute_des_state(start, goal, max_time, t):
des_pos = np.minimum(1., (t / max_time)) * (goal - start) + start
des_vel = np.zeros_like(start)
return des_pos, des_vel
for _ in range(2):
pos, vel, time = robot.reset()
for i in range(2000):
des_pos, des_vel = compute_des_state(robot.home_pos, goal_state,
3., time)
# works similar to openai gym but also has a pd(+g) controller, so you can
# specify desired positions, velocities and feed forward torques.
pos, vel, time = robot.step(des_pos, des_vel)
def main():
robot = MjWam4()
print(robot.get_transform('world', 'wam/links/base'))