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)