# from abr_control.arms import ur5 as arm
from abr_control.arms import jaco2 as arm
# from abr_control.arms import onejoint as arm
from abr_control.controllers import Sliding
from abr_control.interfaces import CoppeliaSim
# initialize our robot config
robot_config = arm.Config()
# instantiate controller
# NOTE: These values are non-optimal
ctrlr = Sliding(robot_config, kd=10.0, lamb=30.00)
# create our CoppeliaSim interface
interface = CoppeliaSim(robot_config, dt=0.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)
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)
interface.connect()
# set up lists for tracking data
ee_track = []
target_track = []
obstacle_track = []
moving_obstacle = True
obstacle_xyz = np.array([0.09596, -0.2661, 0.64204])
try:
# get visual position of end point of object
feedback = interface.get_feedback()
start = robot_config.Tx("EE", q=feedback["q"])
"""
import traceback
import numpy as np
from abr_control.arms import jaco2 as arm
from abr_control.controllers import Floating
from abr_control.interfaces import CoppeliaSim
# initialize our robot config
robot_config = arm.Config()
# instantiate the controller
ctrlr = Floating(robot_config, dynamic=False, task_space=False)
# create the CoppeliaSim interface and connect up
interface = CoppeliaSim(robot_config, dt=0.005)
interface.connect()
# set up arrays for tracking end-effector and target position
ee_track = []
q_track = []
try:
# get the end-effector's initial position
feedback = interface.get_feedback()
start = robot_config.Tx("EE", q=feedback["q"])
print("\nSimulation starting...\n")
while 1:
# get joint angle and velocity feedback
# from abr_control.arms import ur5 as arm
from abr_control.arms import jaco2 as arm
# from abr_control.arms import onejoint as arm
from abr_control.controllers import Joint
from abr_control.interfaces import CoppeliaSim
# initialize our robot config
robot_config = arm.Config()
# instantiate the REACH controller for the jaco2 robot
ctrlr = Joint(robot_config, kp=50)
# create interface and connect
interface = CoppeliaSim(robot_config=robot_config, dt=0.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) * 0.3
# For CoppeliaSim 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)
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,
means=[3.14, 3.14],
variances=[1.57, 1.57],
)
# create our CoppeliaSim interface
interface = CoppeliaSim(robot_config, dt=0.005)
interface.connect()
interface.send_target_angles(q=robot_config.START_ANGLES)
# 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
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# create opreational space controller
ctrlr_dof = np.array([True, False, False, True, True, True])
ctrlr = OSC(
robot_config,
kp=200,
null_controllers=[damping],
vmax=[10, 10], # [m/s, rad/s]
# control (x, alpha, beta, gamma) out of [x, y, z, alpha, beta, gamma]
ctrlr_dof=ctrlr_dof,
)
# create our interface
interface = CoppeliaSim(robot_config, dt=0.005)
interface.connect()
# set up lists for tracking data
ee_track = []
ee_angles_track = []
target_track = []
target_angles_track = []
try:
count = 0
print("\nSimulation starting...\n")
while 1:
# get arm feedback
feedback = interface.get_feedback()
robot_config = arm.Config()
# damp the movements of the arm
damping = Damping(robot_config, kv=10)
# instantiate controller
ctrlr = OSC(
robot_config,
kp=200,
null_controllers=[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)
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.3])
interface.set_xyz(name="target", xyz=target_xyz)
# 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 = CoppeliaSim(robot_config, dt=0.005)
interface.connect()
# pregenerate our path and orientation planners
n_timesteps = 100
traj_planner = path_planners.SecondOrderDMP(error_scale=50,
n_timesteps=n_timesteps)
orientation_planner = path_planners.Orientation()
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