def test_dJ(): test_arm = TwoJoint() robot_config = arm.Config() 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.dJ('link0', q, dq), test_arm.dJ_link0(q, dq)) assert np.allclose( robot_config.dJ('joint0', q, dq), test_arm.dJ_joint0(q, dq)) assert np.allclose( robot_config.dJ('link1', q, dq), test_arm.dJ_link1(q, dq)) assert np.allclose( robot_config.J('joint1', q), test_arm.J_joint1(q)) assert np.allclose( robot_config.J('link2', q), test_arm.J_link2(q)) assert np.allclose( robot_config.J('EE', q), test_arm.J_EE(q))
def test_g(): test_arm = TwoJoint() robot_config = arm.Config() 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_generate_and_save_function(): robot_config = arm.Config(use_cython=True) # remove any saved functions from cache folder if os.path.isfile(robot_config.config_folder + '/EE_T/setup.py'): shutil.rmtree(robot_config.config_folder + '/EE_T') # generate a function robot_config.T('EE', q=np.zeros(robot_config.N_JOINTS)) # confirm file has been generated assert os.path.isfile(robot_config.config_folder + '/EE_T/setup.py')
def test_C(): test_arm = TwoJoint() robot_config = arm.Config() 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_J(): test_arm = TwoJoint() robot_config = arm.Config() 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.J("link0", q), test_arm.J_link0(q)) assert np.allclose(robot_config.J("joint0", q), test_arm.J_joint0(q)) assert np.allclose(robot_config.J("link1", q), test_arm.J_link1(q)) assert np.allclose(robot_config.J("joint1", q), test_arm.J_joint1(q)) assert np.allclose(robot_config.J("link2", q), test_arm.J_link2(q)) assert np.allclose(robot_config.J("EE", q), test_arm.J_EE(q))
def test_load_from_file(): robot_config = arm.Config(use_cython=True) # generate a function robot_config.T('EE', q=np.zeros(robot_config.N_JOINTS)) # if returning expression, function should be None expression, function = robot_config._load_from_file('EE_T', lambdify=False) assert expression is not None assert function is None # if returning function, expression should be None expression, function = robot_config._load_from_file('EE_T', lambdify=True) assert expression is None assert function is not None
def test_R(): test_arm = TwoJoint() robot_config = arm.Config() 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('link0', q), test_arm.R_link0(q)) assert np.allclose(robot_config.R('joint0', q), test_arm.R_joint0(q)) assert np.allclose(robot_config.R('link1', q), test_arm.R_link1(q)) assert np.allclose(robot_config.R('joint1', q), test_arm.R_joint1(q)) assert np.allclose(robot_config.R('link2', q), test_arm.R_link2(q)) assert np.allclose(robot_config.R('EE', q), test_arm.R_EE(q))
""" Running the threelink arm with the pygame display. The arm will move the end-effector to the target, which can be moved by clicking on the background. """ import numpy as np # from abr_control.arms import threejoint as arm from abr_control.arms import twojoint as arm from abr_control.interfaces import PyGame from abr_control.controllers import OSC # initialize our robot config robot_config = arm.Config(use_cython=True) # create our arm simulation arm_sim = arm.ArmSim(robot_config) # create an operational space controller ctrlr = OSC(robot_config, kp=20, vmax=None, use_C=True, use_g=False) def on_click(self, mouse_x, mouse_y): self.target[0] = self.mouse_x self.target[1] = self.mouse_y # create our interface interface = PyGame(robot_config, arm_sim, dt=.001, on_click=on_click) interface.connect() # create a target