def jaco():
I = Interface()
I.connect()
# ------------------------Jaco---------------------------#
ROBOT_URDF_PATH = '../models/jaco_pkg/jaco_description/urdf/test_jaco.urdf'
BLOCK_URDF = "../ss-pybullet-master/models/drake/objects/block_for_pick_and_place_mid_size.urdf"
END_EFFECTOR_LINK = 'jaco_eef_link'
TABLE = '../models/table/table.urdf'
CUBE_URDF = '../models/cube/%s_cube.urdf'
TRAY_URDF = '../models/tray/tray.urdf'
robot = I.load_model(ROBOT_URDF_PATH)
redBlock1 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.22, -0.2, 0.265)), fixed_base = False)
redBlock2 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.35, 0.05, 0.265)), fixed_base = False)
greenBlock1 = I.load_model(CUBE_URDF % 'green', Pose(Point(0.38, -0.1, 0.2599)), fixed_base = False)
greenBlock2 = I.load_model(CUBE_URDF % 'green', Pose(Point(0.38, 0.2, 0.2599)), fixed_base = False)
table = I.load_model(TABLE, Pose(Point(0.35, 0.0, 0.05), Euler(0.0, 0.0, 1.57)), fixed_base = False, scaling = 0.3)
# ------------------------Jaco---------------------------#
velocities = Csv('JACO_velocities.csv',
['joints_3', 'joints_5', 'joints_7', 'joints_9', 'joints_11', 'joints_13', 'joints_16',
'joints_19', 'joints_22'], mode = 'r').read()
for joint, values in velocities.items():
velocities[joint] = map(float, values)
velocities = np.vstack(velocities.values())
I.replay(robot, velocities.T)
time.sleep(2.0)
I.disconnect()
def __init__(self):
rospy.init_node('torque')
self.headers = ['joints%d' % i
for i in range(6)] + ['timestep_sec', 'timestep_nsec']
self.filename = '/media/mtb/Data Disk/JACO/DEMO/STACKING/DEMO_0/torques/torques_ur5.csv'
self.writer = Csv(self.filename, self.headers, 'a')
rospy.Subscriber('/torques', Torques, self.torque_callback)
rospy.spin()
def read_files_descriptor():
file_downloader = Csv(database_connector)
database = Dataset(database_connector, file_downloader)
return jsonify({
MESSAGE_RESULT: database.get_files(request.args.get("type"))
}), HTTP_STATUS_CODE_SUCCESS
def delete_file(filename):
file_downloader = Csv(database_connector)
database = Dataset(database_connector, file_downloader)
thread_pool = ThreadPoolExecutor()
thread_pool.submit(database.delete_file, filename)
return jsonify({MESSAGE_RESULT:
MESSAGE_DELETED_FILE}), HTTP_STATUS_CODE_SUCCESS
class Torque:
def __init__(self):
rospy.init_node('torque')
self.headers = ['joints%d' % i
for i in range(6)] + ['timestep_sec', 'timestep_nsec']
self.filename = '/media/mtb/Data Disk/JACO/DEMO/STACKING/DEMO_0/torques/torques_ur5.csv'
self.writer = Csv(self.filename, self.headers, 'a')
rospy.Subscriber('/torques', Torques, self.torque_callback)
rospy.spin()
def torque_callback(self, msg):
timestep_sec = float(msg.header.stamp.secs)
timestep_nsec = float(msg.header.stamp.nsecs)
torques = msg.data
data = list(torques) + [timestep_sec, timestep_nsec]
self.writer.write([data])
def read_files(filename):
file_downloader = Csv(database_connector)
database = Dataset(database_connector, file_downloader)
limit, skip, query = 20, 0, {}
request_params = request.args.to_dict()
if "limit" in request_params:
if int(request_params["limit"]) < PAGINATE_FILE_LIMIT:
limit = int(request_params["limit"])
if "skip" in request_params:
if int(request_params["skip"]) >= 0:
skip = int(request_params["skip"])
if "query" in request_params:
query = request_params["query"]
file_result = database.read_file(filename, skip, limit, query)
return jsonify({MESSAGE_RESULT: file_result}), HTTP_STATUS_CODE_SUCCESS
def create_file():
url = request.json[URL_FIELD_NAME]
filename = request.json[FILENAME]
request_errors = analyse_request_errors(request_validator, filename, url)
if request_errors is not None:
return request_errors
file_downloader = Csv(database_connector)
database = Dataset(database_connector, file_downloader)
database.add_file(url, filename)
return (
jsonify({
MESSAGE_RESULT:
f'{MICROSERVICE_URI_GET}{request.json[FILENAME]}'
f'{MICROSERVICE_URI_GET_PARAMS}'
}),
HTTP_STATUS_CODE_SUCCESS_CREATED,
)
import numpy as np import vrep import time import os from utils import Csv filename = 'data/torques.csv' #simulation timestep dt = .001 csv_reader = Csv(filename).read() data = np.array([map(float, val) for val in csv_reader.values()]) data_times = data[10:, 0:] secs = [data_times[0][i + 1] - data_times[0][i] for i in range(700)] nsecs = [data_times[1][i + 1] - data_times[1][i] for i in range(700)] #Interval of time for time simulation timestep = np.array(secs) + np.array(nsecs) / 1e09 #Torques values for replay torques = [data[:6, i] for i in range(200)] #Velocity of each joint is set to 10000.0 to allow the joint to just use the torque force velocity = np.ones(6) * 9000 #Change the sign of the velocity if we have negative torques # velocity = np.multiply(velocity,np.sign([data[:6,i] for i in range(200)]))
def ur5():
I = Interface()
I.connect()
MOVABLE_ARM_JOINTS = [1, 2, 3, 4, 5, 6]
ROBOT_URDF_PATH = '../models/ur5_description/urdf/ur5.urdf'
END_EFFECTOR_LINK = 'end_effector_link'
TABLE = '../models/table/table.urdf'
CUBE_URDF = '../models/cube/%s_cube.urdf'
# ------------------------UR5--------------------------#
x_offset = 0.33
y_offset = 0.15
robot = I.load_model(ROBOT_URDF_PATH)
redBlock1 = I.load_model(CUBE_URDF % 'red',
Pose(Point(0.3 + x_offset, -0.1 + y_offset,
0.265)),
fixed_base=False)
redBlock2 = I.load_model(CUBE_URDF % 'red',
Pose(
Point(0.35 + x_offset, 0.05 + y_offset,
0.265)),
fixed_base=False)
table = I.load_model(TABLE,
Pose(Point(0.35 + x_offset, 0.0 + y_offset, 0.05),
Euler(0.0, 0.0, 1.57)),
fixed_base=False,
scaling=0.3)
# ------------------------Jaco---------------------------#
velocities = Csv('UR5_velocities.csv', [
'joints_1', 'joints_2', 'joints_3', 'joints_4', 'joints_5', 'joints_6',
'joints_11', 'joints_13', 'joints_15', 'joints_16', 'joints_17',
'joints_18'
],
mode='r').read()
torques = Csv('UR5_torques.csv', [
'joints_1', 'joints_2', 'joints_3', 'joints_4', 'joints_5', 'joints_6',
'joints_11', 'joints_13', 'joints_15', 'joints_16', 'joints_17',
'joints_18'
],
mode='r').read()
action = Csv('UR5_action.csv', ['action'], mode='r').read()
for joint, values in velocities.items():
velocities[joint] = map(float, values)
velocities = np.vstack(velocities.values())
for joint, values in torques.items():
torques[joint] = map(float, values)
torques = np.vstack(torques.values())
I.replay(robot, [velocities.T, torques.T, action.values()[0]])
#
time.sleep(5.0)
I.disconnect()