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')
while 1:
# get joint angle and velocity feedback
feedback = interface.get_feedback()
# calculate the control signal
u = ctrlr.generate(q=feedback['q'], dq=feedback['dq'])
# send forces into VREP
interface.send_forces(u)
# calculate the position of the hand
hand_xyz = robot_config.Tx('EE', q=feedback['q'])
# track end effector position
ee_track.append(hand_xyz)
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
feedback = interface.get_feedback()
# calculate the control signal
u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"])
# send forces into CoppeliaSim
interface.send_forces(u)
# calculate the position of the hand
hand_xyz = robot_config.Tx("EE", q=feedback["q"])
# track end effector position
ee_track.append(np.copy(hand_xyz))
q_track.append(np.copy(feedback["q"]))
except:
print(traceback.format_exc())
finally:
# close the connection to the arm
# create the VREP interface and connect up
interface = VREP(robot_config, dt=.001)
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'])
while 1:
# get joint angle and velocity feedback
feedback = interface.get_feedback()
# calculate the control signal
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
)
# send forces into VREP
interface.send_forces(u)
# calculate the position of the hand
hand_xyz = robot_config.Tx('EE', q=feedback['q'])
# track end effector position
ee_track.append(hand_xyz)
except:
print(traceback.format_exc())
finally:
# close the connection to the arm
interface.disconnect()
import numpy as np
import traceback
import time
import abr_jaco2
from abr_control.controllers import Floating
# initialize our robot config
robot_config = abr_jaco2.Config(
use_cython=True)
ctrlr = Floating(robot_config, dynamic=True, task_space=True)
# run controller once to generate functions / take care of overhead
# outside of the main loop, because force mode auto-exits after 200ms
zeros = np.zeros(robot_config.N_JOINTS)
ctrlr.generate(zeros, zeros)
# create our interface for the jaco2
interface = abr_jaco2.Interface(robot_config)
q_track = []
u_track = []
q_T_track = []
# connect to the jaco
interface.connect()
interface.init_position_mode()
# Move to home position
interface.send_target_angles(robot_config.START_ANGLES)
try: