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