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()
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' BASKET_URDF = '../models/basket/basket.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) basket = I.load_model(BASKET_URDF, Pose(Point(0.35, 0.05, 0.265)), 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('velocities_UR5.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('torques_UR5.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() 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() torques = Csv('JACO_torques.csv', [ 'joints_3', 'joints_5', 'joints_7', 'joints_9', 'joints_11', 'joints_13', 'joints_16', 'joints_19', 'joints_22' ], mode='r').read() action = Csv('JACO_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(2.0) I.disconnect()
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)]))