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 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
from abr_control.interfaces.mujoco import Mujoco
if len(sys.argv) > 1:
arm_model = sys.argv[1]
else:
arm_model = "jaco2"
# initialize our robot config for the jaco2
robot_config = arm(arm_model)
# create interface and connect
interface = Mujoco(robot_config=robot_config, dt=0.001)
interface.connect()
interface.send_target_angles(robot_config.START_ANGLES)
# instantiate the REACH controller for the jaco2 robot
ctrlr = Joint(robot_config, kp=20, kv=10)
# make the target an offset of the current configuration
feedback = interface.get_feedback()
target = feedback["q"] + np.random.random(robot_config.N_JOINTS) * 2 - 1
# set up arrays for tracking end-effector and target position
q_track = []
try:
count = 0
print("\nSimulation starting...\n")
while count < 2500:
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
from abr_control.controllers import Joint, path_planners
from abr_control.utils import transformations
# change this flag to False to use position control
use_force_control = True
# initialize our robot config
robot_config = arm.Config()
# create our arm simulation
arm_sim = arm.ArmSim(robot_config)
if use_force_control:
# create an operational space controller
ctrlr = Joint(robot_config, kp=100, kv=30)
# create our path planner
n_timesteps = 2000
path_planner = path_planners.InverseKinematics(robot_config)
# create our interface
dt = 0.001
interface = PyGame(robot_config, arm_sim, dt=dt)
interface.connect()
feedback = interface.get_feedback()
try:
print("\nSimulation starting...")
print("Click to move the target.\n")
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 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=.001)
interface.connect()
# make the target an offset of the current configuration
feedback = interface.get_feedback()
target_pos = 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
from abr_control.utils import transformations
from abr_control.utils.transformations import quaternion_conjugate, quaternion_multiply
# initialize our robot config for the jaco2
robot_config = arm("mujoco_balljoint.xml", folder=".", use_sim_state=False)
# create our Mujoco interface
interface = Mujoco(robot_config, dt=0.001)
interface.connect()
# instantiate controller
kp = 100
kv = np.sqrt(kp)
ctrlr = Joint(
robot_config,
kp=kp,
kv=kv,
quaternions=[True],
)
# set up lists for tracking data
q_track = []
target_track = []
error_track = []
target_geom_id = interface.sim.model.geom_name2id("target")
green = [0, 0.9, 0, 0.5]
red = [0.9, 0, 0, 0.5]
threshold = 0.002 # threshold distance for being within target before moving on
# get the end-effector's initial position
np.random.seed(0)
from abr_control.interfaces import PyGame
from abr_control.controllers import Joint, path_planners
from abr_control.utils import transformations
# change this flag to False to use position control
use_force_control = True
# initialize our robot config
robot_config = arm.Config(use_cython=True)
# create our arm simulation
arm_sim = arm.ArmSim(robot_config)
if use_force_control:
# create an operational space controller
ctrlr = Joint(robot_config, kp=100, kv=30)
# create our path planner
n_timesteps = 2000
path_planner = path_planners.InverseKinematics(robot_config)
# create our interface
dt = 0.001
interface = PyGame(robot_config, arm_sim, dt=dt)
interface.connect()
feedback = interface.get_feedback()
try:
print('\nSimulation starting...')
print('Click to move the target.\n')