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 = []