def test_scaling(): robot_config = ur5.Config(use_cython=True) # looking at data centered around 50 with range of 25 each way means = np.ones(robot_config.N_JOINTS) * 50 variances = np.ones(robot_config.N_JOINTS) * 25 for ii in range(50): # test without spherical conversion adapt = DynamicsAdaptation( robot_config.N_JOINTS, robot_config.N_JOINTS, means=means, variances=variances, spherical=False, ) unscaled = np.random.random(robot_config.N_JOINTS) * 50 + 25 scaled = adapt.scale_inputs(unscaled) assert np.all(-1 < scaled) and np.all(scaled < 1) # test spherical conversion adapt = DynamicsAdaptation( robot_config.N_JOINTS, robot_config.N_JOINTS, means=means, variances=variances, spherical=True, ) unscaled = np.random.random(robot_config.N_JOINTS) * 50 + 25 scaled = adapt.scale_inputs(unscaled) assert np.linalg.norm(scaled) - 1 < 1e-5
""" A basic script for connecting to the arm and putting it in floating mode, which only compensates for gravity. The end-effector position is recorded and plotted when the script is exited (with ctrl-c). """ import numpy as np import traceback from abr_control.arms import ur5 as arm # from abr_control.arms import jaco2 as arm # from abr_control.arms import onelink as arm from abr_control.controllers import Floating from abr_control.interfaces import VREP # initialize our robot config robot_config = arm.Config(use_cython=True) # if using the Jaco 2 arm with the hand attached, use the following instead: # robot_config = arm.Config(use_cython=True, hand_attached=True) # instantiate the controller ctrlr = Floating(robot_config, dynamic=True) # create the VREP interface and connect up interface = VREP(robot_config, dt=.005) interface.connect() # set up arrays for tracking end-effector and target position ee_track = [] try: feedback = interface.get_feedback()
""" Move the UR5 CoppeliaSim arm to a target position while avoiding an obstacle. The simulation ends after 1500 time steps, and the trajectory of the end-effector is plotted in 3D. """ import numpy as np from abr_control.arms import ur5 as arm # from abr_control.arms import jaco2 as arm from abr_control.controllers import OSC, AvoidObstacles, Damping from abr_control.interfaces import CoppeliaSim # initialize our robot config robot_config = arm.Config() avoid = AvoidObstacles(robot_config) # damp the movements of the arm damping = Damping(robot_config, kv=10) # instantiate the REACH controller with obstacle avoidance ctrlr = OSC( robot_config, kp=200, null_controllers=[avoid, 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], ) # create our CoppeliaSim interface interface = CoppeliaSim(robot_config, dt=0.005)