def test_Tx():
test_arm = TwoJoint()
# set up the Mujoco interface and config
robot_config = arm("twojoint")
interface = Mujoco(robot_config=robot_config, visualize=False)
interface.connect()
q_vals = np.linspace(0, 2 * np.pi, 50)
for q0 in q_vals:
for q1 in q_vals:
q = [q0, q1]
assert np.allclose(
robot_config.Tx("link0", q, object_type="geom"),
test_arm.Tx_link0(q),
atol=1e-5,
)
assert np.allclose(
robot_config.Tx("joint0", q, object_type="joint"),
test_arm.Tx_joint0(q),
atol=1e-5,
)
assert np.allclose(
robot_config.Tx("link1", q, object_type="geom"),
test_arm.Tx_link1(q),
atol=1e-5,
)
assert np.allclose(
robot_config.Tx("joint1", q, object_type="joint"),
test_arm.Tx_joint1(q))
assert np.allclose(robot_config.Tx("link2", q, object_type="geom"),
test_arm.Tx_link2(q))
assert np.allclose(robot_config.Tx("EE", q), test_arm.Tx_EE(q))
def test_g():
test_arm = TwoJoint()
# set up the Mujoco interface and config
robot_config = arm("twojoint")
interface = Mujoco(robot_config=robot_config, visualize=False)
interface.connect()
q_vals = np.linspace(0, 2 * np.pi, 50)
for q0 in q_vals:
for q1 in q_vals:
q = [q0, q1]
assert np.allclose(robot_config.g(q), test_arm.g(q))
def test_C():
test_arm = TwoJoint()
robot_config = arm("twojoint")
interface = Mujoco(robot_config=robot_config, visualize=False)
interface.connect()
with pytest.raises(NotImplementedError):
q_vals = np.linspace(0, 2 * np.pi, 15)
for q0 in q_vals:
for q1 in q_vals:
q = [q0, q1]
for dq0 in q_vals:
for dq1 in q_vals:
dq = [dq0, dq1]
assert np.allclose(robot_config.C(q, dq),
test_arm.C(q, dq))
def test_R():
test_arm = TwoJoint()
robot_config = arm("twojoint")
interface = Mujoco(robot_config=robot_config, visualize=False)
interface.connect()
q_vals = np.linspace(0, 2 * np.pi, 50)
for q0 in q_vals:
for q1 in q_vals:
q = [q0, q1]
assert np.allclose(robot_config.R("link1", q),
test_arm.R_link1(q),
atol=1e-5)
assert np.allclose(robot_config.R("link2", q),
test_arm.R_link2(q),
atol=1e-5)
assert np.allclose(robot_config.R("EE", q),
test_arm.R_EE(q),
atol=1e-5)
import sys
import traceback
import numpy as np
import glfw
from abr_control.controllers import OSC, Damping, signals
from abr_control.arms.mujoco_config import MujocoConfig as arm
from abr_control.interfaces.mujoco import Mujoco
from abr_control.utils import transformations
if len(sys.argv) > 1:
arm_model = sys.argv[1]
else:
arm_model = "jaco2"
# initialize our robot config for the jaco2
robot_config = arm(arm_model)
# create our Mujoco interface
interface = Mujoco(robot_config, dt=0.001)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# instantiate controller
ctrlr = OSC(
robot_config,
kp=200,
null_controllers=[damping],
vmax=[0.5, 0], # [m/s, rad/s]
# control (x, y, z) out of [x, y, z, alpha, beta, gamma]
mode, which only compensates for gravity. The end-effector position
is recorded and plotted when the script is exited (with ctrl-c).
In this example, the floating controller is applied in the joint space
"""
import numpy as np
import traceback
import glfw
from abr_control.controllers import Floating
from abr_control.arms.mujoco_config import MujocoConfig as arm
from abr_control.interfaces.mujoco import Mujoco
from abr_control.utils import transformations
# initialize our robot config
robot_config = arm('jaco2')
# instantiate the controller
ctrlr = Floating(robot_config, task_space=True, dynamic=True)
# create the Mujoco interface and connect up
interface = Mujoco(robot_config, dt=.001)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
# set up arrays for tracking end-effector and target position
ee_track = []
q_track = []
try:
# get the end-effector's initial position
"""
Running the joint controller with an inverse kinematics path planner
for a Mujoco simulation. The path planning system will generate
a trajectory in joint space that moves the end effector in a straight line
to the target, which changes every n time steps.
"""
import numpy as np
import glfw
from abr_control.interfaces.mujoco import Mujoco
from abr_control.arms.mujoco_config import MujocoConfig as arm
from abr_control.controllers import path_planners
from abr_control.utils import transformations
# initialize our robot config for the jaco2
robot_config = arm("ur5", use_sim_state=False)
# create our path planner
n_timesteps = 2000
path_planner = path_planners.InverseKinematics(robot_config)
# create our interface
dt = 0.001
interface = Mujoco(robot_config, dt=dt)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
feedback = interface.get_feedback()
try:
print("\nSimulation starting...")
print("Click to move the target.\n")
The simulation ends after 1500 time steps, and the
trajectory of the end-effector is plotted in 3D.
"""
import traceback
import glfw
import numpy as np
from abr_control.arms.mujoco_config import MujocoConfig as arm
from abr_control.controllers import Joint
from abr_control.interfaces.mujoco import Mujoco
from abr_control.utils import transformations
from abr_control.utils.transformations import quaternion_conjugate, quaternion_multiply
# initialize our robot config for the jaco2
robot_config = arm("mujoco_balljoint.xml", folder=".", use_sim_state=False)
# create our Mujoco interface
interface = Mujoco(robot_config, dt=0.001)
interface.connect()
# instantiate controller
kp = 100
kv = np.sqrt(kp)
ctrlr = Joint(
robot_config,
kp=kp,
kv=kv,
quaternions=[True],
)
"""
Move the jao2 Mujoco arm to a target position.
The simulation ends after 1500 time steps, and the
trajectory of the end-effector is plotted in 3D.
"""
import traceback
import numpy as np
import glfw
from abr_control.controllers import OSC
from abr_control.arms.mujoco_config import MujocoConfig as arm
from abr_control.interfaces.mujoco import Mujoco
from abr_control.utils import transformations
# initialize our robot config for the jaco2
robot_config = arm("mujoco_balljoint.xml", folder=".")
# create our Mujoco interface
interface = Mujoco(robot_config, dt=0.001)
interface.connect()
# instantiate controller
ctrlr = OSC(
robot_config,
kp=200,
# control (x, y, z) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, True, False, False, False],
)
# set up lists for tracking data
ee_track = []
