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)
if N_JOINTS == 1:
model_filename = 'onejoint'
ctrlr_dof = [True, False, False, False, False, False]
elif N_JOINTS == 2:
model_filename = 'twojoint'
ctrlr_dof = [True, True, False, False, False, False]
elif N_JOINTS == 3:
model_filename = 'threejoint'
ctrlr_dof = [True, True, True, False, False, False]
else:
raise Exception("Only 1-3 joint arms are available in this example")
robot_config = MujocoConfig(model_filename)
dt = 0.001
interface = Mujoco(robot_config, dt=dt)
interface.connect()
ctrlr = OSC(robot_config, kp=10, kv=5, ctrlr_dof=ctrlr_dof)
interface.send_target_angles(np.ones(N_JOINTS))
target = np.array([0.1, 0.1, 0.3, 0, 0, 0])
interface.set_mocap_xyz('target', target[:3])
interface.set_mocap_xyz('hand', np.array([.2, .4, 1]))
q_track = []
count = 0
link_name = 'EE'
else:
arm_model = "jaco2"
# initialize our robot config for the jaco2
robot_config = arm(arm_model)
ctrlr_dof = [True, True, True, False, False, False]
dof_labels = ["x", "y", "z", "a", "b", "g"]
dof_print = f"* DOF Controlled: {np.array(dof_labels)[ctrlr_dof]} *"
stars = "*" * len(dof_print)
print(stars)
print(dof_print)
print(stars)
dt = 0.001
# create our interface
interface = Mujoco(robot_config, dt=dt)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=30,
kv=20,
ko=180,
null_controllers=[damping],
vmax=[10, 10], # [m/s, rad/s]
# control (x, alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=ctrlr_dof,
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
feedback = interface.get_feedback()
start = robot_config.Tx('EE', q=feedback['q'])
print('\nSimulation starting...\n')
while 1:
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")
count = 0
while 1:
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
if count % n_timesteps == 0:
import numpy as np
import traceback
import glfw
from abr_control.arms.mujoco_config import MujocoConfig as arm
from abr_control.interfaces.mujoco import Mujoco
from abr_control.controllers import Joint
# initialize our robot config
robot_config = arm('jaco2')
# instantiate the REACH controller for the jaco2 robot
ctrlr = Joint(robot_config, kp=20, kv=10)
# create interface and connect
interface = Mujoco(robot_config=robot_config, dt=.001)
interface.connect()
# interface.send_target_angles(robot_config.START_ANGLES)
# make the target an offset of the current configuration
feedback = interface.get_feedback()
target = feedback['q'] + np.random.random(robot_config.N_JOINTS) * 2 - 1
# set up arrays for tracking end-effector and target position
q_track = []
try:
count = 0
print('\nSimulation starting...\n')
while count < 1500:
if interface.viewer.exit:
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=30,
kv=20,
ko=180,
null_controllers=[damping],
vmax=[10, 10], # [m/s, rad/s]
# control (x, alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=ctrlr_dof)
# create our interface
interface = Mujoco(robot_config, dt=.001)
interface.connect()
target_orientation = np.random.random(4)
target_orientation /= np.linalg.norm(target_orientation)
target_xyz = np.array([0.3, 0.3, 0.5])
# set up lists for tracking data
ee_track = []
ee_angles_track = []
target_track = []
target_angles_track = []
try:
count = 0
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]
ctrlr_dof=[True, True, True, False, False, False],
)
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],
)
# set up lists for tracking data
q_track = []
target_track = []
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 = []
target_track = []
target_geom_id = interface.sim.model.geom_name2id("target")
print(
"To plan the path based on real time instead of steps, append"
+ " 'True' to your script call"
)
if len(sys.argv) > 1:
show_plot = sys.argv[1]
else:
show_plot = False
model_filename = "threejoint"
robot_config = MujocoConfig(model_filename)
dt = 0.001
interface = Mujoco(robot_config, dt=dt)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
ctrlr = OSC(
robot_config, kp=30, kv=20, ctrlr_dof=[True, True, True, False, False, False]
)
interface.send_target_angles(np.ones(3))
target_xyz = np.array([0.1, 0.1, 0.3])
interface.set_mocap_xyz("target", target_xyz)
interface.set_mocap_xyz("hand", np.array([0.2, 0.4, 1]))
target_geom_id = interface.sim.model.geom_name2id("target")
from abr_control.controllers import OSC, Damping, path_planners
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
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 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)
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=30, # position gain
kv=20,
ko=180, # orientation gain
null_controllers=[damping],
vmax=None, # [m/s, rad/s]
# control all DOF [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, True, True, True, True],
if N_JOINTS == 1:
model_filename = "onejoint"
ctrlr_dof = [True, False, False, False, False, False]
elif N_JOINTS == 2:
model_filename = "twojoint"
ctrlr_dof = [True, True, False, False, False, False]
elif N_JOINTS == 3:
model_filename = "threejoint"
ctrlr_dof = [True, True, True, False, False, False]
else:
raise Exception("Only 1-3 joint arms are available in this example")
robot_config = MujocoConfig(model_filename)
dt = 0.001
interface = Mujoco(robot_config, dt=dt)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
ctrlr = OSC(robot_config, kp=10, kv=5, ctrlr_dof=ctrlr_dof)
interface.send_target_angles(np.ones(N_JOINTS))
target = np.array([0.1, 0.1, 0.3, 0, 0, 0])
interface.set_mocap_xyz("target", target[:3])
interface.set_mocap_xyz("hand", np.array([0.2, 0.4, 1]))
q_track = []
count = 0
# initialize our robot config
robot_config = arm('jaco2')
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=200, # position gain
ko=200, # orientation gain
null_controllers=[damping],
# control (alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=[False, False, False, True, True, True])
# create our interface
interface = Mujoco(robot_config, dt=.001)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
# set up lists for tracking data
ee_angles_track = []
target_angles_track = []
try:
print('\nSimulation starting...\n')
def get_random_orientation():
rand_orient = np.random.rand(4)
rand_orient /= np.linalg.norm(rand_orient)
return rand_orient
from abr_control.controllers import OSC, Damping, RestingConfig, path_planners
from abr_control.interfaces.mujoco import Mujoco
from abr_control.arms.mujoco_config import MujocoConfig
use_wall_clock = True
if len(sys.argv) > 1:
show_plot = sys.argv[1]
else:
show_plot = False
model_filename = 'threejoint'
robot_config = MujocoConfig(model_filename)
dt = 0.001
interface = Mujoco(robot_config, dt=dt)
interface.connect()
ctrlr = OSC(robot_config,
kp=10,
kv=5,
ctrlr_dof=[True, True, True, False, False, False])
interface.send_target_angles(np.ones(3))
target_xyz = np.array([0.1, 0.1, 0.3])
interface.set_mocap_xyz('target', target_xyz)
interface.set_mocap_xyz('hand', np.array([.2, .4, 1]))
# create our path planner
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=30, # position gain
kv=20,
ko=180, # orientation gain
null_controllers=[damping],
vmax=None, # [m/s, rad/s]
# control all DOF [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, True, True, True, True])
# create our interface
interface = Mujoco(robot_config, dt=.001)
interface.connect()
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
def get_target(robot_config):
# pregenerate our path and orientation planners
n_timesteps = 2000
traj_planner = path_planners.BellShaped(error_scale=0.01,
n_timesteps=n_timesteps)
starting_orientation = robot_config.quaternion('EE', feedback['q'])
target_orientation = np.random.random(3)
