def step(self, action):
if self.interface is None:
self.interface = VREP(robot_config=self.robot_config, dt=self.dt)
self.interface.connect()
if self.requested_target_shadow_q is not None:
self._set_target_shadow(self.requested_target_shadow_q)
self.requested_target_shadow_q = None
if self.requested_reset_q is not None:
self._reset_angles(self.requested_reset_q)
self.requested_reset_q = None
# Send torq to VREP
self.interface.send_forces(action)
feedback = self.interface.get_feedback()
# Get joint angle and velocity feedback
q = feedback['q']
dq = feedback['dq']
# Concatenate q and dq
state = np.hstack((q, dq)) # (12,)
reward = 0.0
if self.close_requested:
self._close()
self.close_requested = False
return state, reward
def __init__(self, direct=False):
self.robot_config = arm.Config(use_cython=False, hand_attached=True)
# Timestap is fixed and specified in .ttt file
self.dt = 0.005
self.direct = direct
if direct:
self.interface = VREP(robot_config=self.robot_config, dt=self.dt)
self.interface.connect()
else:
self.interface = None
self.requested_reset_q = None
self.requested_target_shadow_q = None
self.close_requested = False
ctrlr_dof=[True, True, True, False, False, False])
# create our adaptive controller
adapt = signals.DynamicsAdaptation(
n_neurons=1000,
n_ensembles=5,
n_input=2, # we apply adaptation on the most heavily stressed joints
n_output=2,
pes_learning_rate=5e-5,
intercepts_bounds=[-0.6, -0.2],
intercepts_mode=-0.2,
means=[3.14, 3.14],
variances=[1.57, 1.57])
# create our VREP interface
interface = VREP(robot_config, dt=.005)
interface.connect()
# set up lists for tracking data
ee_track = []
target_track = []
# get Jacobians to each link for calculating perturbation
J_links = [
robot_config._calc_J('link%s' % ii, x=[0, 0, 0])
for ii in range(robot_config.N_LINKS)
]
try:
# get the end-effector's initial position
feedback = interface.get_feedback()
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()
start = robot_config.Tx('EE', q=feedback['q'])
# run ctrl.generate once to load all functions
zeros = np.zeros(robot_config.N_JOINTS)
ctrlr.generate(q=zeros, dq=zeros)
robot_config.R('EE', q=zeros)
print('\nSimulation starting...\n')
from abr_control.arms import jaco2 as arm
# from abr_control.arms import onelink as arm
from abr_control.controllers import Sliding
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 controller
# NOTE: These values are non-optimal
ctrlr = Sliding(robot_config, kd=10.0, lamb=30.00)
# create our VREP interface
interface = VREP(robot_config, dt=.001)
interface.connect()
# set up lists for tracking data
ee_track = []
target_track = []
try:
# get the end-effector's initial position
feedback = interface.get_feedback()
start = robot_config.Tx('EE', feedback['q'])
# make the target offset from that start position
target_xyz = start + np.array([0.2, -0.2, 0.0])
interface.set_xyz(name='target', xyz=target_xyz)
# run ctrl.generate once to load all functions
robot_config = arm.Config(use_cython=True)
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create opreational space controller
ctrlr = OSC(
robot_config,
kp=100, # position gain
ko=250, # 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 = VREP(robot_config, dt=.005)
interface.connect()
# pregenerate our path and orientation planners
n_timesteps = 1000
traj_planner = path_planners.BellShaped(error_scale=50,
n_timesteps=n_timesteps)
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
starting_orientation = robot_config.quaternion('EE', feedback['q'])
target_orientation = np.random.random(3)
target_orientation /= np.linalg.norm(target_orientation)
# convert our orientation to a quaternion
target_orientation = [0] + list(target_orientation)
# 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 Joint
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=False)
# instantiate the REACH controller for the jaco2 robot
ctrlr = Joint(robot_config, kp=50)
# create interface and connect
interface = VREP(robot_config=robot_config, dt=.005)
interface.connect()
# make the target an offset of the current configuration
feedback = interface.get_feedback()
target = feedback['q'] + np.ones(robot_config.N_JOINTS) * .3
# For VREP files that have a shadow arm to show the target configuration
# get joint handles for shadow
names = ['joint%i_shadow' % ii for ii in range(robot_config.N_JOINTS)]
joint_handles = []
for name in names:
interface.get_xyz(name) # this loads in the joint handle
joint_handles.append(interface.misc_handles[name])
# move shadow to target position
interface.send_target_angles(target, joint_handles)
from abr_control.interfaces import VREP
import nengo
from models import poppy_config
from models import ndmps_fs_rhythmic_spa_frontend_poppybot
importlib.reload(ndmps_fs_rhythmic_spa_frontend_poppybot)
from models.ndmps_fs_rhythmic_spa_frontend_poppybot import generate
input_signals = ['WALKING', 'RUNNING', 'GALLOP']
# input_signals = [" + ".join(x) for x in
# itertools.combinations(input_signals, 2)]
# print(input_signals)
poppy = poppy_config.Config()
interface = VREP(poppy)
interface.connect()
interface.disconnect()
interface.connect()
interface.send_target_angles(np.zeros(6))
# for input_signal in input_signals:
#
input_signal = 'WALKING'
print(input_signal)
model = nengo.Network(seed=10)
with model:
ndmps_r = generate(input_signal=input_signal)
def vrep_func(t, x):
# convert to radians
# if using the Jaco 2 arm with the hand attached, use the following instead:
# robot_config = arm.Config(use_cython=True, hand_attached=False)
# 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 = VREP(robot_config, dt=.005)
interface.connect()
# set up lists for tracking data
ee_angles_track = []
target_angles_track = []
try:
print('\nSimulation starting...\n')
while 1:
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
target = np.hstack(
[interface.get_xyz('target'),
from abr_control.arms import ur5 as arm
# from abr_control.arms import jaco2 as arm
from abr_control.controllers import OSC, signals
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=False)
# instantiate the REACH controller with obstacle avoidance
ctrlr = OSC(robot_config, kp=200, vmax=0.5)
avoid = signals.AvoidObstacles(robot_config)
# create our VREP interface
interface = VREP(robot_config, dt=.001)
interface.connect()
# set up lists for tracking data
ee_track = []
target_track = []
obstacle_track = []
try:
num_targets = 0
back_to_start = False
# get visual position of end point of object
feedback = interface.get_feedback()
# set up the values to be used by the Jacobian for the object end effector
start = robot_config.Tx('EE', q=feedback['q'])
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
'''
import numpy as np
from abr_control.arms import ur5 as arm
from abr_control.interfaces import VREP
from abr_control.utils import transformations
# initialize our robot config
robot_config = arm.Config(use_cython=True)
# create our interface
interface = VREP(robot_config, dt=.005)
interface.connect()
# specify which parameters [x, y, z, alpha, beta, gamma] to control
# NOTE: needs to be an array to properly select elements of J and u_task
ctrlr_dof = np.array([True, True, True, True, True, True])
# control gains
kp = 300
ko = 300
kv = np.sqrt(kp+ko) * 1.5
orientations = [
[0, 0, 0],
[np.pi/4, np.pi/4, np.pi/4],
[-np.pi/4, -np.pi/4, np.pi/2],
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
'''
import numpy as np
from abr_control.arms import ur5 as arm
from abr_control.interfaces import VREP
from abr_control.utils import transformations
# initialize our robot config
robot_config = arm.Config(use_cython=True)
# create our interface
interface = VREP(robot_config, dt=.005)
interface.connect()
# specify which parameters [x, y, z, alpha, beta, gamma] to control
# NOTE: needs to be an array to properly select elements of J and u_task
ctrlr_dof = np.array([False, False, False, True, True, True])
# control gains
kp = 300
ko = 300
kv = np.sqrt(kp+ko)
orientations = [
[0, 0, 0],
[np.pi/4, np.pi/4, np.pi/4],
[-np.pi/4, -np.pi/4, np.pi/2],
class VREPEnv(object):
def __init__(self, direct=False):
self.robot_config = arm.Config(use_cython=False, hand_attached=True)
# Timestap is fixed and specified in .ttt file
self.dt = 0.005
self.direct = direct
if direct:
self.interface = VREP(robot_config=self.robot_config, dt=self.dt)
self.interface.connect()
else:
self.interface = None
self.requested_reset_q = None
self.requested_target_shadow_q = None
self.close_requested = False
@property
def u_dim(self):
return self.robot_config.N_JOINTS
@property
def x_dim(self):
return self.robot_config.N_JOINTS * 2
def reset_angles(self, q):
if self.direct:
self._reset_angles(q)
else:
self.requested_reset_q = q
def _reset_angles(self, q):
self.interface.send_target_angles(q)
def set_target_shadow(self, q, direct=False):
if self.direct:
self._set_target_shadow(q)
else:
self.requested_target_shadow_q = q
def _set_target_shadow(self, q):
names = [
'joint%i_shadow' % i for i in range(self.robot_config.N_JOINTS)
]
joint_handles = []
for name in names:
self.interface.get_xyz(name) # this loads in the joint handle
joint_handles.append(self.interface.misc_handles[name])
self.interface.send_target_angles(q, joint_handles)
def close(self, direct=False):
if self.direct:
self._close()
else:
self.close_requested = True
def _close(self):
self.interface.disconnect()
def step(self, action):
if self.interface is None:
self.interface = VREP(robot_config=self.robot_config, dt=self.dt)
self.interface.connect()
if self.requested_target_shadow_q is not None:
self._set_target_shadow(self.requested_target_shadow_q)
self.requested_target_shadow_q = None
if self.requested_reset_q is not None:
self._reset_angles(self.requested_reset_q)
self.requested_reset_q = None
# Send torq to VREP
self.interface.send_forces(action)
feedback = self.interface.get_feedback()
# Get joint angle and velocity feedback
q = feedback['q']
dq = feedback['dq']
# Concatenate q and dq
state = np.hstack((q, dq)) # (12,)
reward = 0.0
if self.close_requested:
self._close()
self.close_requested = False
return state, reward
