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)
