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()