def stop(self):
#set divers to stop mode
force = 0
err_code = vrep.simxSetJointForce(self.clientID,
self.left_front_handle, force,
vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID, self.left_back_handle,
force, vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID,
self.right_back_handle, force,
vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID,
self.right_front_handle, force,
vrep.simx_opmode_oneshot)
force = 10
for h in self.side_handles:
err_code = vrep.simxSetJointForce(self.clientID, h, force,
vrep.simx_opmode_oneshot)
#break
self.leftvelocity = 10
self.rightvelocity = 10
vrep.simxSetJointTargetVelocity(self.clientID, self.left_front_handle,
self.leftvelocity,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.left_back_handle,
self.leftvelocity,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.right_back_handle,
self.rightvelocity,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.right_front_handle,
self.rightvelocity,
vrep.simx_opmode_streaming)
def step_simulation(self, torque_commands):
vrep.simxSetJointTargetVelocity(
self.clientID, self.jointHandles[0],
90000 if torque_commands[0] > 0 else -9000,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(
self.clientID, self.jointHandles[0], torque_commands[0]
if torque_commands[0] > 0 else -torque_commands[0],
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(
self.clientID, self.jointHandles[1],
90000 if torque_commands[1] > 0 else -9000,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(
self.clientID, self.jointHandles[1], torque_commands[1]
if torque_commands[1] > 0 else -torque_commands[1],
vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(
self.clientID
) # Trigger next simulation step (Blocking function call)
vrep.simxGetPingTime(
self.clientID
) # Ensure simulation step has completed (Blocking function call)
def open_hand(
self): #this should work to place gripper at initial open position
_, dist = vrep.simxGetJointPosition(self.client_id, self.motor_handle, \
vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.client_id, self.motor_handle, 20,
vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, -0.5, \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
# start_time = time.time()
while dist > -1e-06: #8.22427227831e-06:#-1.67e-06: # Block until gripper is fully open
_, dist = vrep.simxGetJointPosition(self.client_id, self.motor_handle, \
vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, -0.5, \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
# end_time = time.time()
# if (end_time-start_time > 2):
# self.restart()
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, 0.0, \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def close_hand(self):
vrep.simxSetJointForce(self.client_id, self.motor_handle, 100, \
vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, \
0.5, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def set_joint_position(theta, clientID, jointHandles):
[
base_handle, joint_one_handle, joint_two_handle, joint_three_handle,
joint_four_handle, joint_five_handle, joint_six_handle
] = jointHandles
'''
for jointHandle in jointHandles[1:]:
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.01, vrep.simx_opmode_oneshot)
'''
for jointHandle in jointHandles[1:]:
vrep.simxSetJointForce(clientID, jointHandle, 27,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta[0],
vrep.simx_opmode_oneshot)
time.sleep(timeBetweenJointMovements)
vrep.simxSetJointTargetPosition(clientID, joint_two_handle, theta[1],
vrep.simx_opmode_oneshot)
time.sleep(timeBetweenJointMovements)
vrep.simxSetJointTargetPosition(clientID, joint_three_handle, theta[2],
vrep.simx_opmode_oneshot)
time.sleep(timeBetweenJointMovements)
vrep.simxSetJointTargetPosition(clientID, joint_four_handle, theta[3],
vrep.simx_opmode_oneshot)
time.sleep(timeBetweenJointMovements)
vrep.simxSetJointTargetPosition(clientID, joint_five_handle, theta[4],
vrep.simx_opmode_oneshot)
time.sleep(timeBetweenJointMovements)
vrep.simxSetJointTargetPosition(clientID, joint_six_handle, theta[5],
vrep.simx_opmode_oneshot)
time.sleep(timeBetweenJointMovements)
def step(self, torque):
torque = torque / 20.0 + 0.05
# apply torque
vrep.simxSetJointForce(self.clientID, self.motor, torque,
vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.clientID)
# get information
[res,
angle] = vrep.simxGetObjectOrientation(self.clientID, self.shelf, -1,
vrep.simx_opmode_buffer)
currentAngle = angle[2]
error = currentAngle - self.targetAngle
[res, dist] = vrep.simxGetObjectPosition(self.clientID, self.ball, -1,
vrep.simx_opmode_buffer)
if dist[1] > self.threshold:
self.done = True
else:
self.done = False
# reward = float(2*np.exp(-(torque[0]-0.03)*(torque[0]-0.03)/0.0005))
# if torque > 0.02:
# reward -= float(20.0*(torque - 0.02))
if self.done == True:
reward -= 100.0
if self.t > 0.5:
self.done = True
#update
self.t = self.t + self.dt
#print(self.t, torque)
return [error,
torque], reward, self.done, [torque, currentAngle, dist[1]]
def setTorques(self, torques):
vrep.simxPauseCommunication(self.id, True)
for j, t in zip(self.joints, torques):
vrep.simxSetJointTargetVelocity(self.id, j,
np.sign(t) * 1e10,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(self.id, j, np.abs(t),
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.id, False)
def handle_input(self, values):
# Set the velocity to some large number with the correct sign,
# because v-rep is weird like that
vrep.simxSetJointTargetVelocity(self.cid, self.arm_joint, values[0]*100,
vrep.simx_opmode_oneshot)
# Apply the desired torques to the joints
# V-REP is looking for just the absolute value here
vrep.simxSetJointForce(self.cid, self.arm_joint, abs(values[0]),
vrep.simx_opmode_oneshot)
def go(self):
#set divers to go mode
force =10
err_code = vrep.simxSetJointForce(self.clientID, self.left_front_handle, force, vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID, self.left_back_handle, force, vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID, self.right_back_handle, force, vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID, self.right_front_handle, force, vrep.simx_opmode_oneshot)
force =0
for h in self.side_handles:
err_code = vrep.simxSetJointForce(self.clientID, h, force, vrep.simx_opmode_oneshot)
def setJointForce(self,
jointIndex,
force,
isHandJoint=False,
useSignalToJoint=0):
if (isHandJoint):
jointSignalPrefixes = GB_CROBOT_HAND_JOINT_SIGNAL_NAME_PREFIXES
else:
jointSignalPrefixes = GB_CROBOT_JOINT_SIGNAL_NAME_PREFIXES
# Signal to the joint control callback script
jointSignalName = jointSignalPrefixes[jointIndex] + "Force"
if (useSignalToJoint):
res = vrep.simxSetFloatSignal(
self._clientID, jointSignalName, force,
vrep.simx_opmode_oneshot_wait
) # set the signal value: !!simx_opmode_oneshot_wait
else:
res = vrep.simxSetJointForce(
self._clientID,
self._jointHandles[jointIndex],
force, # force to apply
vrep.simx_opmode_blocking)
if (res != 0):
print('Set Joint Force Error', res)
return res
def set_forces(self, force_vec):
forces = force_vec.flatten().tolist()
for handle_idx in range(0, self.joint_count):
_ = vrep.simxSetJointForce(self.client_id,
self.handles[handle_idx],
forces[handle_idx],
vrep.simx_opmode_oneshot)
def _set_joint_effort(
self, U
): # set torque U = [u1, u2, u3, u4, u5, u6, u7] to 7 joints in V-REP
torque = [
vrep.simxGetJointForce(self.clientID, self.handles['joint'][i],
vrep.simx_opmode_blocking)[1]
for i in range(self.action_dims)
] # retrieve the current torque from 7 joints
if len(U) != self.action_dims:
raise Exception('the dimension of action is wrong.')
# Give the torque and targeted velocity to joints.
for i in range(self.action_dims):
if U[i] > self._max_torque: # limit the torque of each joint under max_torque
U[i] = self._max_torque
if np.sign(torque[i]) * np.sign(U[i]) < 0:
self.joint_target_velocities[i] *= -1
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.handles['joint'][i],
self.joint_target_velocities[i], vrep.simx_opmode_blocking)
# Just in case
if _ != 0:
print('set target velocity error %s' % _)
raise Exception('failed to set target velocity to joints.')
_ = vrep.simxSetJointForce(self.clientID, self.handles['joint'][i],
abs(U[i]), vrep.simx_opmode_blocking)
# Just in case
if _ != 0:
print('set torques error %s' % _)
raise Exception('failed to set torques to joints.')
def set_forces(force_vec):
_ = vrep.simxPauseCommunication(self.client_id, True)
for handle_idx in range(0, self.joint_count):
_ = vrep.simxSetJointForce(self.client_id,
self.handles[handle_idx],
force_vec[handle_idx],
vrep.simx_opmode_blocking)
_ = vrep.simxPauseCommunication(self.client_id, False)
def full_stop(self):
errorCode = vrep.simxSetJointForce(self.kwargs['clientID'],
self.kwargs.get('motor'),
self.kwargs['REST_TORQUE'],
vrep.simx_opmode_blocking)
errorCode = vrep.simxSetJointTargetVelocity(self.kwargs['clientID'],
self.kwargs.get('motor'),
0,
vrep.simx_opmode_blocking)
def __init__(self,clientID):
motors =['motor1','motor2','motor3','motor4','motor5','motor6','motor7']
self.motor_handles = [vrep.simxGetObjectHandle(clientID,name, vrep.simx_opmode_blocking)[1] for name in motors]
sensors = ['head_sensor','sensor1r','sensor1l','sensor2r','sensor2l','sensor3r','sensor3l','sensor4r','sensor4l', 'sensor5r','sensor5l','sensor6r','sensor6l','sensor7r','sensor7l','sensor8r','sensor8l']
self.sensor_handles = [vrep.simxGetObjectHandle(clientID, sensor, vrep.simx_opmode_blocking)[1] for sensor in sensors]
obstacles = ['Obj0','Obj1','Obj2','Obj3','Obj4','Obj5','Obj6','Obj7','Obj8','Obj9','Obj10']
self.obst_handles = [vrep.simxGetObjectHandle(clientID, obj, vrep.simx_opmode_blocking)[1] for obj in obstacles]
body_modules = ['snake_v2', 'module2', 'module3', 'module4', 'module5', 'module6', 'module7', 'module8']
self.body_module_handles = [vrep.simxGetObjectHandle(clientID, obj, vrep.simx_opmode_blocking)[1] for obj in body_modules]
self.snake_body = vrep.simxGetObjectHandle(clientID, 'snake_v2', vrep.simx_opmode_blocking)[1]
print ("\nSet motor torques...\n")
for motor_handle in self.motor_handles:
_=vrep.simxSetJointForce(clientID,motor_handle,5,vrep.simx_opmode_blocking)
if _ !=0: raise Exception()
self.setMotor(clientID, 0, 1)
print ("\nAssign sensor values\n")
for sensor_handle in self.sensor_handles:
_,_,_,_,_ = vrep.simxReadProximitySensor(clientID, sensor_handle, vrep.simx_opmode_streaming)
print ("\nSetting Object postions...\n")
self.obst_count = r.randint(5,8)
for i in range (self.obst_count):
obst = r.choice(self.obst_handles)
_, [obst_copy] = vrep.simxCopyPasteObjects(clientID, [obst], vrep.simx_opmode_blocking)
x = round(r.uniform(-5,8),3)
y = round(r.uniform(-7.5,8),3)
_ = vrep.simxSetObjectPosition(clientID, obst_copy, -1, (x, y, 0.25),vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectOrientation(clientID, obst_copy, obst_copy, (r.randint(0,180),0,0), vrep.simx_opmode_oneshot)
#_ = vrep.simxSetObjectPosition(clientID, self.snake_body, -1, (-9.5, 10, 0.085), vrep.simx_opmode_oneshot)
#---------------------------------------------Initialising the motor position buffer
self.modulepos = list()
for motor_handle in self.motor_handles:
_,moduleCord = vrep.simxGetObjectPosition(clientID,motor_handle,-1,vrep.simx_opmode_streaming)
self.modulepos.append(moduleCord)
if _ !=1 :
print (_)
raise Exception()
#---------------------------------------------Initialising body_modules collision buffer
for module in self.body_module_handles:
_,collision_state = vrep.simxReadCollision(clientID, module, vrep.simx_opmode_streaming)
print _
def open_hand(self):#this should work to place gripper at initial open position
_, dist = vrep.simxGetJointPosition(self.client_id, \
self.motor_handle, vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.client_id, self.motor_handle, \
20, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, \
-0.5, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
start_time = time.time()
while dist > -1e-06:#8.22427227831e-06:#-1.67e-06: # Block until gripper is fully open
_, dist = vrep.simxGetJointPosition(self.client_id, \
self.motor_handle, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, \
-0.5, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
if time.time() - start_time > 5:
print(dist)
print('trouble opening gripper')
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, \
0.0, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def setJointForce(clientID, joint_handle, set_joint_val):
vrep.simxSetJointForce(clientID,
joint_handle,
set_joint_val,
oneshot)
def callback(data):
print('inside callback')
data = data.data
#print(data)
#a,b,c = calc_parabola_vertex(data[0], data[1], data[2], data[3], data[4], data[5])
a,b = calc_straight_line(data[0], data[1], data[4], data[5])
print([a,b])
z_req_vision= ((a*(x_req))+(b))/2
# z_req_vision= (a*(x_req**2))+(b*x_req)+c
print('z required from vision'+str(z_req_vision))
print('x required from vision'+str(x_req))
print('-'*8+'Checking if ball can be blocked'+'-'*8)
transformation = np.array([[1,0,0,x_req*1000],[0,1,0,0],[0,0,1,z_req_vision*1000],[0,0,0,1]])
desired_angles = robot.inverseKinematics(transformation)
if desired_angles:
print('-'*8+'Moving'+'-'*8)
error = desired_angles-angles
#error = np.array(error)
error_collect.append(error)
#error_collect.append(error.reshape(3,1))
sum = np.sum(np.abs(error))
Kp = [1000*(sum-np.abs(i))/sum for i in error]
Kv = 100
velocity_error = np.array([0 ,0 ,0])
desired_velocity = np.copy(velocity_error)
for i in range(3):
while abs(error[i])>epsilon:
vrep.simxSetJointTargetVelocity(clientID,joints[i],error[i]*9999/abs(error[i]),vrep.simx_opmode_oneshot)
# if error[i]>0.5:
# vrep.simxSetJointForce(clientID,joints[i],abs(Kp*0.5)+(Kv*velocity_error[i]),vrep.simx_opmode_oneshot)
# else:#+(Kv*velocity_error[i])
if abs(error[i])<epsilon:
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],abs(Kp[i]*(error[i]))+Kv*velocity_error[i],vrep.simx_opmode_oneshot)
error[i] = desired_angles[i] - (vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
#print(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
if abs(error[i])<epsilon:
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
res,velocity = vrep.simxGetObjectFloatParameter(clientID,joints[i],2012,vrep.simx_opmode_blocking)
velocity_error[i] = desired_velocity[i] - velocity
#error_collect.append(error)
#print(error)
# while np.sum(np.abs(error))>epsilon:
# for i in range(6):
# if abs(error[i])<epsilon/6.:
# vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
# vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
# else:
# vrep.simxSetJointTargetVelocity(clientID,joints[i],error[i]*9999/abs(error[i]),vrep.simx_opmode_oneshot)
# vrep.simxSetJointForce(clientID,joints[i],abs(Kp*(error[i])),vrep.simx_opmode_oneshot)
# error[i] = desired_angles[i] - (vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
a = []
for i in range(3):
vrep.simxSetJointTargetVelocity(clientID,joints[i],0,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999,vrep.simx_opmode_oneshot)
a.append(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
# print(a)
else:
for i in range(3):
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
print('hi')
sub_MTML.unregister()
# get handles for the 2 gripper joints
err,gripperJoint1Handle=vrep.simxGetObjectHandle(clientID,"youBotGripperJoint1",vrep.simx_opmode_oneshot_wait)
printErr(err, 'Get handle for gripper joint 1: {}'.format(gripperJoint1Handle), 'Error by getting handle for gripper joint 1: {}'.format(err))
err,gripperJoint2Handle=vrep.simxGetObjectHandle(clientID,"youBotGripperJoint2",vrep.simx_opmode_oneshot_wait)
printErr(err, 'Get handle for gripper joint 2: {}'.format(gripperJoint2Handle), 'Error by getting handle for gripper joint 2: {}'.format(err))
# get handles for the 2 attached cameras
err,visionSensor1Handle=vrep.simxGetObjectHandle(clientID,"Vision_sensor1",vrep.simx_opmode_oneshot_wait)
printErr(err, 'Get handle for vision sensor 1: {}'.format(visionSensor1Handle), 'Error by getting handle for vision sensor 1: {}'.format(err))
err,visionSensor2Handle=vrep.simxGetObjectHandle(clientID,"Vision_sensor2",vrep.simx_opmode_oneshot_wait)
printErr(err, 'Get handle for vision sensor 2: {}'.format(visionSensor2Handle), 'Error by getting handle for vision sensor 2: {}'.format(err))
# set forces for the joint(motor)s
vrep.simxSetJointForce(clientID,armJoint0Handle,20,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,armJoint1Handle,20,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,armJoint2Handle,20,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,armJoint3Handle,20,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,armJoint4Handle,20,vrep.simx_opmode_oneshot)
counter = 0
# this loop won't end
while(err == vrep.simx_error_noerror or err == vrep.simx_error_novalue_flag):
counter += 1
print counter
# move the robot somehow
vrep.simxSetJointTargetVelocity(clientID,frontleftMotorHandle,0.5,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID,rearleftMotorHandle,0.5,vrep.simx_opmode_oneshot)
def get_sample_distribution():
# functions = {}
# args = {}
# real_fun(functions)
# 1-15 are examples along a sphere, one at top of sphere, one in middle, one in bottom,
# and then three rings at centred .25d, .5d, .75d with radius = 2
key = 'Traj{}'.format(traj_no)
# state_normalizers = np.zeros((1, 2))
# action_normalizers = np.zeros((1, 2))
# states = np.genfromtxt('trajectories/target/Traj{}state.txt'.format(traj_no), delimiter=',', dtype=np.float32)[1:]
# actions = np.genfromtxt('trajectories/target/Traj{}action.txt'.format(traj_no),delimiter=' ', dtype=np.float32)[1:]
states, actions = get_all_files()
states[isnan(states)] = 0
actions[isnan(actions)] = 0
# print(states.shape)
# print(actions.shape)
N, T, du = np.asarray(actions).shape # Close all threads, in case
_, _, dx = np.asarray(states).shape
#
# ilqr = Control(traj_no, max_iter=3)
# X, U, cost = ilqr.ilqr(states[0,:], actions)
samples = sample_dist(states, actions)
raw_input()
target_vel = X[:,24:]
vrep.simxFinish(-1)
cid = connect()
vrep.simxLoadScene(cid, 'scene/arm_no_control.ttt', 0, mode())
dt = .01
vrep.simxSetFloatingParameter(cid,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
vrep.simxStartSimulation(cid, vrep.simx_opmode_streaming)
joint_names = ['redundantRob_joint' + str(x) for x in range(1, 8)]
joint_handles = [vrep.simxGetObjectHandle(cid,
name,
vrep.simx_opmode_oneshot_wait)[1]
for name in joint_names]
# # get handle for target and set up streaming
_, target_handle = vrep.simxGetObjectHandle(cid,
'redundantRob_manipSphere',
vrep.simx_opmode_oneshot_wait)
#
_, self_handle = vrep.simxGetObjectHandle(cid, 'Rectangle',
vrep.simx_opmode_oneshot_wait)
#
_, target_pos = vrep.simxGetObjectPosition(cid,
target_handle, -1,
vrep.simx_opmode_oneshot_wait)
real_args(args, target_pos)
target_move(cid, target_handle, False, functions[key],
args[key])
for t in range(T):
#
# for j in range(du):
# if np.sign(actions[t,j]) * target_vel[t,j] < 0:
# target_vel[j] = target_vel[j] * -1
[vrep.simxSetJointTargetVelocity(cid, joint_handles[j], target_vel[t,j],
vrep.simx_opmode_streaming) for j in range(du)]
[vrep.simxSetJointForce(cid, joint_handles[j], actions[t,j],
vrep.simx_opmode_streaming) for j in range(du)]
vrep.simxSynchronousTrigger(cid)
raw_input()
print('here')
vrep.simxSynchronousTrigger(cid)
vrep.simxStopSimulation(cid, vrep.simx_opmode_streaming)
vrep.simxFinish(cid)
def main():
# global variables
global velocity
global armJoints
global wheels
global MAX_FORCE
global tcp_handle
global bodyHandle
global proxSensor
global holdingCube
# get transformation data
M = transformation_data.M
S = transformation_data.S
T_b_s = transformation_data.T_b_s
zero_pose = transformation_data.zero_pose
plate_pose = transformation_data.plate_pose
front_pose = transformation_data.front_pose
# VREP stuff
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
# gets handle for TCP - used in printing comparison
e, tcp_handle = vrep.simxGetObjectHandle(clientID,
'youBotGripperJoint1',
vrep.simx_opmode_oneshot_wait)
e, bodyHandle = vrep.simxGetObjectHandle(clientID, "youBot",
vrep.simx_opmode_blocking)
e, cubeHandle = vrep.simxGetObjectHandle(clientID, "Rectangle16",
vrep.simx_opmode_blocking)
e, proxSensor = vrep.simxGetObjectHandle(clientID, "Proximity_sensor",
vrep.simx_opmode_blocking)
#e, visionSensorHandle = vrep.simxGetObjectHandle(clientID, "Vision_sensor", vrep.simx_opmode_blocking)
# initialize wheel motors
e1, wheels[0] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fl',
vrep.simx_opmode_oneshot_wait)
e2, wheels[1] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rl',
vrep.simx_opmode_oneshot_wait)
e3, wheels[2] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rr',
vrep.simx_opmode_oneshot_wait)
e4, wheels[3] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fr',
vrep.simx_opmode_oneshot_wait)
#initialize arm joints
arm_poses = [0, 0, 0, 0, 0]
for i in range(5):
# get object handle
e, armJoints[i] = vrep.simxGetObjectHandle(
clientID, 'youBotArmJoint' + str(i),
vrep.simx_opmode_oneshot_wait)
# set max force
vrep.simxSetJointForce(clientID, armJoints[i], MAX_FORCE,
vrep.simx_opmode_oneshot_wait)
# MAKE A CALL WITH simx_opmode_streaming TO INIT DATA AQUISITION
vrep.simxGetObjectPosition(clientID, bodyHandle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, bodyHandle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectPosition(clientID, bodyHandle, -1,
vrep.simx_opmode_buffer)
vrep.simxGetObjectOrientation(clientID, bodyHandle, -1,
vrep.simx_opmode_buffer)
#IMAGE PROCESSING
res, vsHandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor',
vrep.simx_opmode_oneshot_wait)
res, bsHandle = vrep.simxGetObjectHandle(clientID, 'Has_Block_sensor',
vrep.simx_opmode_oneshot_wait)
res, cubeHandle = vrep.simxGetObjectHandle(
clientID, 'Rectangle14', vrep.simx_opmode_oneshot_wait)
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, vsHandle, 0, vrep.simx_opmode_streaming)
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, bsHandle, 0, vrep.simx_opmode_streaming)
#TESTING if we can move the arm between two poses
'''
DO NOT DELETE
NEEDED TO MAKE ROBOT GET CORRECT POSITION AT START (wait until buffer is cleared)
'''
print("Initializing robot...\n\n")
time.sleep(2)
stopWheels()
moveArm(transformation_data.plate_pose, [])
release()
### INSERT YOUR CODE HERE ###
# MEANS NO CUBE -0.046000331640244
'''
while True:
grab()
release()
'''
continue_running = True
while continue_running: # This will happen as long as the robot is alive
print("Start of main loop")
start_time = time.time()
# STATE 1
#Search for a cube
dist, blob_center = getCubeProperties(clientID, vsHandle)
while (dist < 0):
turnRight(0) #spins
#print(dist)
dist, blob_center = getCubeProperties(clientID, vsHandle)
if (time.time() - start_time >
10): #If we've been searching for 15 seconds
#Give up
continue_running = False
break
stopWheels()
if (not continue_running): #no cubes found by front camera
print("Giving up...")
break
# STATE 2
#Navigate to cube
# TODO: instead of drifting, we want rotation so that it is a lot more natural
# could look at the move to position function
print("Navigating to cube")
while (len(blob_center) != 0 and
blob_center[0] > 140): # MOVE RIGHT UNTIL BLOCK IS CENTERED
moveRight()
dist, blob_center = getCubeProperties(clientID, vsHandle)
while (len(blob_center) != 0 and
blob_center[0] < 110): # MOVE LEFT UNTIL BLOCK IS CENTERED
moveLeft()
dist, blob_center = getCubeProperties(clientID, vsHandle)
while (len(blob_center) != 0 and blob_center[1] < 90
): # MOVE FORWARD UNTIL BLOCK IS WITHIN REACH
moveForward()
dist, blob_center = getCubeProperties(clientID, vsHandle)
stopWheels()
# STATE 3
#Grab cube
print("Grabbing cube")
detectionState, yaw, cube_pose = detectCube()
grabCube_success = grabCube(cube_pose, yaw)
# Check if we have cube
if (grabCube_success):
time.sleep(0.5)
#print('---------Start Debugging!!!!!!!!---------')
dist, blob_center = getRearCubeProperties(clientID, bsHandle)
if (len(blob_center) == 0):
grabCube_success = False
print("Successfully grabbed cube: " + str(grabCube_success))
#STATE 4
#Move to destination
if (grabCube_success):
returnCube()
release()
time.sleep(5)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
def controller_motor(clientID, target_handle, self_handle, joint_handles,
firstPass):
# global firstPass
global joint_target_velocities
global motor_mask
global u
if (firstPass):
joint_target_velocities = np.ones(len(joint_handles)) * 10000.0
u = [1] * len(joint_handles)
motor_mask = [0] * len(joint_handles)
print("OMG FIRSTPASS")
#-- Decide of the motor velocities:
#Grab target
#error, target_pos = vrep.simxGetObjectPosition(cid,target_handle,self_handle,mode())
#Grab joint angles
q = np.zeros(len(joint_handles))
dq = np.zeros(len(joint_handles))
for ii, joint_handle in enumerate(joint_handles):
if (ii < 2):
continue
# get the joint angles
_, q[ii] = vrep.simxGetJointPosition(clientID, joint_handle,
vrep.simx_opmode_oneshot_wait)
if _ != 0: raise Exception()
# get the joint velocity
_, dq[ii] = vrep.simxGetObjectFloatParameter(
clientID,
joint_handle,
2012, # ID for angular velocity of the joint
vrep.simx_opmode_oneshot_wait)
if _ != 0: raise Exception()
# get the current joint torque
_, torque = vrep.simxGetJointForce(clientID, joint_handle,
vrep.simx_opmode_oneshot_wait)
if _ != 0: raise Exception()
joint_target_velocities[ii] = 5
u[ii] = np.random.uniform(-500, 500)
motor_mask[2] = 1
# if force has changed signs,
# we need to change the target velocity sign
vrep.simxPauseCommunication(clientID, 1)
for ii, joint_handle in enumerate(joint_handles):
if np.sign(torque) * np.sign(u[ii]) < 0:
joint_target_velocities[ii] = joint_target_velocities[ii] * -1
if (motor_mask[ii]):
vrep.simxSetJointTargetVelocity(clientID, joint_handle,
joint_target_velocities[ii],
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(
clientID,
joint_handle,
abs(u[ii]), # force to apply
vrep.simx_opmode_oneshot)
if _ != 0: raise Exception()
vrep.simxPauseCommunication(clientID, 0)
firstPass = False
return firstPass
if __name__ == "__main__":
rospy.init_node('points_talker', anonymous=True)
error_collect = []
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to V-REP
joints = [1]*3
angles = np.zeros(3)
for i in range(3):
errorCode,joints[i] = vrep.simxGetObjectHandle(clientID,'IRB140_joint'+str(i+1),vrep.simx_opmode_blocking)
angles[i] =(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
epsilon =6e-2
for i in range(3):
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
# Kp = np.abs(error/np.sum(np.abs(error)))
#Kp = 2000
#Kv = 100
# kp = [2000, 2000,2000]
# kv = [100,100,100]
res, floor_handle = vrep.simxGetObjectHandle(clientID, 'floor_handle', vrep.simx_opmode_blocking)
res, s_handle = vrep.simxGetObjectHandle(clientID, 'kinect', vrep.simx_opmode_blocking)
position = vrep.simxGetObjectPosition(clientID, s_handle, vrep.sim_handle_parent, vrep.simx_opmode_blocking)
orientation = vrep.simxGetObjectOrientation(clientID, s_handle, vrep.sim_handle_parent, vrep.simx_opmode_buffer)
print('-'*8+'Calculating Trajectory'+'-'*8)
# print(position)
# print(orientation)
flag = True
x_req = 0.30 # to be replaced with robot workspace coordinates
#
left_motor_speed = 0 * np.pi / 180
# Set the motor velocities
vrep.simxSetJointTargetVelocity(clientID, motor_handles[0],
left_motor_speed,
vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(clientID, motor_handles[1],
right_motor_speed,
vrep.simx_opmode_blocking)
# mass3_parent = vrep.simxGetObjectParent(clientID, mass_handles[2], vrep.simx_opmode_blocking)[1]
# vrep.simxSetObjectPosition(clientID, mass_handles[2], -1, (x3-0.1*(sim_time-2), y3, z3), vrep.simx_opmode_blocking)
# Set the motor forces to zero - let them freewheel?
for motor in motor_handles:
vrep.simxSetJointForce(clientID, motor, 1e-3,
vrep.simx_opmode_blocking)
else:
#Set the motor forces to zero - let them freewheel?
for motor in motor_handles:
vrep.simxSetJointForce(clientID, motor, 0.01,
vrep.simx_opmode_blocking)
# vrep.simxSetJointTargetVelocity(clientID, motor, 0, vrep.simx_opmode_blocking)
# Get the spring/damper forces
_, force = vrep.simxGetJointForce(clientID, joint_handles[0],
vrep.simx_opmode_blocking)
print('Time: {:0.2f} \t Spring Force: {:0.2f}'.format(
sim_time, force))
# Get the position of the boddy at each timestep and print it out,
if _ !=0 : raise Exception()
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) <= 0:
joint_target_velocities[ii] = \
joint_target_velocities[ii] * -1
vrep.simxSetJointTargetVelocity(clientID,
joint_handle,
joint_target_velocities[ii], # target velocity
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# and now modulate the force
vrep.simxSetJointForce(clientID,
joint_handle,
abs(u[ii]), # force to apply
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# move simulation ahead one time step
vrep.simxSynchronousTrigger(clientID)
count += dt
else:
raise Exception('Failed connecting to remote API server')
except Exception, e:
print e
finally:
# stop the simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Before closing the connection to V-REP,
# Obtain initial pos
e, jpos = vrep.simxGetJointPosition(client_id, joint,
vrep.simx_opmode_blocking)
print "Current position = %0.3f degrees" % ((jpos * 1.0) / math.pi * 180.0)
# Set custom dt
# _ = vrep.simxSetFloatingParameter(client_id, vrep.sim_floatparam_simulation_time_step, 0.01, vrep.simx_opmode_blocking)
# Synchronous
for i in range(0, 1000):
# Compute and Set torque
e = vrep.simxSetJointTargetVelocity(client_id, joint, 9999.0,
vrep.simx_opmode_blocking)
if e != 0: raise Exception()
e = vrep.simxSetJointForce(client_id, joint, 1,
vrep.simx_opmode_blocking)
if e != 0: raise Exception()
# e = vrep.simxSetJointPosition(client_id, joint, i/180. * math.pi, vrep.simx_opmode_blocking)
# Send Synchronous trigger
e = vrep.simxSynchronousTrigger(client_id)
# Wait for exec to complete
# e = vrep.simxGetPingTime(client_id)
# Obtain position
e, jpos = vrep.simxGetJointPosition(client_id, joint,
vrep.simx_opmode_blocking)
print "Current position = %0.3f degrees" % (
(jpos * 1.0) / math.pi * 180.0)
def set_max_torque(self, torque):
# Set Maximum torque for a joint
rc = vrep.simxSetJointForce(clientID, self.handle, torque, oneshot)
elif sim_time < 5:
right_motor_speed = 0 * np.pi/180
left_motor_speed = 0 * np.pi/180
# Set the motor velocities
vrep.simxSetJointTargetVelocity(clientID, motor_handles[0], left_motor_speed, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(clientID, motor_handles[1], right_motor_speed, vrep.simx_opmode_blocking)
# mass3_parent = vrep.simxGetObjectParent(clientID, mass_handles[2], vrep.simx_opmode_blocking)[1]
# vrep.simxSetObjectPosition(clientID, mass_handles[2], -1, (x3-0.1*(sim_time-2), y3, z3), vrep.simx_opmode_blocking)
# Set the motor forces to zero - let them freewheel?
for motor in motor_handles:
vrep.simxSetJointForce(clientID, motor, 1e-3, vrep.simx_opmode_blocking)
else:
#Set the motor forces to zero - let them freewheel?
for motor in motor_handles:
vrep.simxSetJointForce(clientID, motor, 0.01, vrep.simx_opmode_blocking)
# vrep.simxSetJointTargetVelocity(clientID, motor, 0, vrep.simx_opmode_blocking)
# Get the spring/damper forces
_, force = vrep.simxGetJointForce(clientID, joint_handles[0], vrep.simx_opmode_blocking)
print('Time: {:0.2f} \t Spring Force: {:0.2f}'.format(sim_time, force))
# Get the position of the boddy at each timestep and print it out,
# -1 in this call means absolute position
_, (x, y, z) = vrep.simxGetObjectPosition(clientID, mass_handles[0], -1, vrep.simx_opmode_blocking)
print('Mass 1 -- x: {:5.2f} \t y: {:5.2f} \t z: {:5.2f}'.format(x, y, z))
vrep.simx_opmode_oneshot_wait)
print "getting joint information"
## gets force and position of joints
errorCode1, initPosition1 = vrep.simxGetJointPosition(clientID, joint1handle, vrep.simx_opmode_oneshot_wait)
errorCode1, initForce1 = vrep.simxGetJointForce(clientID, joint1handle, vrep.simx_opmode_oneshot_wait)
errorCode2, initPosition2 = vrep.simxGetJointPosition(clientID, joint2handle, vrep.simx_opmode_oneshot_wait)
errorCode2, initForce2 = vrep.simxGetJointForce(clientID, joint2handle, vrep.simx_opmode_oneshot_wait)
errorCode1, initPosition4 = vrep.simxGetJointPosition(clientID, motor1handle, vrep.simx_opmode_oneshot_wait)
print "set joint information"
## pause stream and prepare dataload to send
vrep.simxPauseCommunication(clientID, True)
errorCode2 = vrep.simxSetJointTargetVelocity(clientID, joint1handle, 0.2, vrep.simx_opmode_oneshot)
errorCode2 = vrep.simxSetJointForce(clientID, joint1handle, 15, vrep.simx_opmode_oneshot)
errorCode2 = vrep.simxSetJointTargetVelocity(clientID, joint2handle, 0.2, vrep.simx_opmode_oneshot)
errorCode2 = vrep.simxSetJointForce(clientID, joint2handle, 15, vrep.simx_opmode_oneshot)
## send data stream
vrep.simxPauseCommunication(clientID, False)
print "move arms"
## move arms
errorCode2 = vrep.simxSetJointTargetPosition(clientID, joint2handle, 200*3.14195/180, vrep.simx_opmode_oneshot_wait)
errorCode3, Position4 = vrep.simxGetJointPosition(clientID, joint2handle, vrep.simx_opmode_oneshot_wait)
targetVelMotor1 = 0
if initPosition4 > 0.0:
targetVelMotor1 = -0.2
else:
targetVelMotor1 = 0.2
errorCode4 = vrep.simxSetJointTargetVelocity(clientID, motor1handle, targetVelMotor1, vrep.simx_opmode_streaming)
def joint_torque(cid, handle, val):
vrep.simxSetJointTargetVelocity(cid, handle, val * 100, vrep_mode)
vrep.simxSetJointForce(cid, handle, abs(val), vrep_mode)
def send_forces(self, u):
""" Apply the specified torque to the robot joints
Apply the specified torque to the robot joints, move the simulation
one time step forward, and update the position of the hand object.
Parameters
----------
u : np.array
the torques to apply to the robot
"""
# invert because VREP torque directions are opposite of expected
u *= -1
for ii, joint_handle in enumerate(self.joint_handles):
# get the current joint torque
_, torque = vrep.simxGetJointForce(
self.clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error retrieving joint torque, ' +
'return code ', _)
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) <= 0:
self.joint_target_velocities[ii] *= -1
_ = vrep.simxSetJointTargetVelocity(
self.clientID,
joint_handle,
self.joint_target_velocities[ii],
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error setting joint target velocity, ' +
'return code ', _)
# and now modulate the force
_ = vrep.simxSetJointForce(self.clientID,
joint_handle,
abs(u[ii]), # force to apply
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error setting max joint force, ' +
'return code ', _)
# Update position of hand object
hand_xyz = self.robot_config.Tx(name='EE', q=self.q)
self.set_xyz('hand', hand_xyz)
# Update orientation of hand object
quaternion = self.robot_config.orientation('EE', q=self.q)
angles = transformations.euler_from_quaternion(
quaternion, axes='rxyz')
self.set_orientation('hand', angles)
# move simulation ahead one time step
vrep.simxSynchronousTrigger(self.clientID)
self.count += self.dt
def sim_robot(robotType,funcType,reloadrobotsim,robDataFileName,\
Tmax=0.,inpfn=None,trialclamp=False,Tperiod=0.,Tclamp=0.,simdt=0.001):
if reloadrobotsim:
with contextlib.closing(
shelve.open(robDataFileName+'_'+robotType+'.shelve', 'r')
) as data_dict:
robtrange = data_dict['robtrange']
rateEvolveProbe = data_dict['angles']
#torqueArray = data_dict['torque'] # no need to load this
else:
if 'robot2' in funcType:
N = 4 # 4-dim system
joint_names = ['shoulder', 'elbow']
if 'rr_' in funcType:
N =7
joint_names = ['s0','s1','e0','e1','w0','w1','w2'] # 7-dim system
else:
N = 2 # 2-dim system
joint_names = ['shoulder']
zerosNby2 = np.zeros(N//2)
###################################### VREP robot arm #####################################
if robotType == 'V-REP':
robotdt = .02 # we run the robot simulator faster as vrep is slow and dynamics is smooth
# and interpolate when feeding to nengo
import vrep
# close any open connections
vrep.simxFinish(-1)
# Connect to the V-REP continuous server
portnum = 19997
clientID = vrep.simxStart('127.0.0.1', portnum, True, True, 500, 5)
if clientID != -1: # if we connected successfully
print ('Connected to V-REP remote API server on port',portnum)
# --------------------- Setup the simulation
vrep.simxSynchronous(clientID,True)
# get handles to each joint
joint_handles = [vrep.simxGetObjectHandle(clientID,
name, vrep.simx_opmode_blocking)[1] for name in joint_names]
# set vrep time step
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step,
robotdt,
vrep.simx_opmode_oneshot)
# start simulation in blocking mode
vrep.simxStartSimulation(clientID,
vrep.simx_opmode_blocking)
simrun = True
robtrange = np.arange(0,Tmax,robotdt)
rateEvolveProbe = np.zeros(shape=(len(robtrange),N))
torqueArray = np.zeros(shape=(len(robtrange),N//2))
for it,t in enumerate(robtrange):
torquet = inpfn(t)[N//2:] # zeros to dq/dt, torque to dv/dt
torqueArray[it,:] = torquet
if trialclamp and (t%Tperiod)>(Tperiod-Tclamp):
if simrun:
# stop the vrep simulation
vrep.simxStopSimulation(clientID,
vrep.simx_opmode_blocking)
simrun = False
rateEvolveProbe[it,:N//2] = zerosNby2
rateEvolveProbe[it,N//2:] = zerosNby2
else:
if not simrun:
# start simulation in blocking mode
vrep.simxStartSimulation(clientID,
vrep.simx_opmode_blocking)
simrun = True
# apply the torque to the vrep arm
# vrep has a crazy way of setting the torque:
# the torque is applied in target velocity direction
# until target velocity is reached,
# so we need to set the sign of the target velocity correct,
# with its value very high so that it is never reached,
# and then set the torque magnitude as desired.
for ii,joint_handle in enumerate(joint_handles):
# first we set the target velocity sign same as the torque sign
_ = vrep.simxSetJointTargetVelocity(clientID,
joint_handle,
np.sign(torquet[ii])*9e4, # target velocity
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# second we set the torque to abs value desired
vrep.simxSetJointForce(clientID,
joint_handle,
abs(torquet[ii]), # 2D torque to apply i.e. \vec{u}(t)
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# step vrep simulation by a time step
vrep.simxSynchronousTrigger(clientID)
# get updated joint angles and velocity from vrep
q = np.zeros(len(joint_handles)) # 2D output \vec{x}(t)
v = np.zeros(len(joint_handles))
for ii,joint_handle in enumerate(joint_handles):
# get the joint angles
_, q[ii] = vrep.simxGetJointPosition(clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# get the joint velocity
_, v[ii] = vrep.simxGetObjectFloatParameter(clientID,
joint_handle,
2012, # parameter ID for angular velocity of the joint
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
rateEvolveProbe[it,:N//2] = q
rateEvolveProbe[it,N//2:] = v
if it%1000==0:
print(it,'time steps, i.e.',t,'s of vrep sim are over.')
# stop the vrep simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# send a blocking command, so that all previous commands finish
# redundant perhaps, since stop simulation is also blocking
vrep.simxGetPingTime(clientID)
# close the V-REP connection
vrep.simxFinish(clientID)
else:
raise Exception('Connection to V-REP remote API server failed')
sys.exit(1)
myarm = None
######################################### PENDULUM ARM ######################################
elif robotType == 'pendulum':
robotdt = simdt # pendulum simulation is fast enough, no need of slower robotdt
if funcType in ('robot1_gravity','robot1XY_gravity'):
from arm_1link_gravity import evolveFns,armXY,armAngles
elif funcType in ('robot1_gravity_interpol','robot1XY_gravity_interpol'):
from arm_1link_gravity_interpol import evolveFns,armXY,armAngles
elif funcType in ('robot2_gravity_interpol'):
from arm_2link_gravity_interpol import evolveFns,armXY,armAngles
elif funcType == 'robot2_todorov':
from arm_2link_todorov import evolveFns,armXY,armAngles
elif funcType in ('robot2_todorov_gravity','robot2XY_todorov_gravity') or 'rr_' in funcType:
from arm_2link_todorov_gravity import evolveFns,armXY,armAngles
elif funcType in ('acrobot2_gravity','acrobot2XY_gravity'):
from acrobot_2link import evolveFns,armXY,armAngles
else:
raise Exception('Choose one- or two-link robot')
if 'rr_' in funcType:
csv_data = pd.read_csv('../dataset/baxter_traj.csv')
robtrange = csv_data['time'].values
else: robtrange = np.arange(0,Tmax,robotdt)
torqueArray = np.zeros(shape=(len(robtrange),N//2))
qzero = np.zeros(N//2) # angles and angvelocities at start are subtracted below
dqzero = np.zeros(N//2) # start from zero, fully downward position initially
q = np.copy(qzero) # must copy, else pointer is used
dq = np.copy(dqzero)
if 'XY' in funcType: # for XY robot, return (positions,angvelocities), though angles are evolved
rateEvolveProbe = np.zeros(shape=(len(robtrange),N+N//2))
def set_robot_state(angles,velocities):
rateEvolveProbe[it,:N] = armXY(angles)
rateEvolveProbe[it,N:] = (velocities-dqzero)
else: # for usual robot return (angles,angvelocities)
rateEvolveProbe = np.zeros(shape=(len(robtrange),N))
def set_robot_state(angles,velocities):
#rateEvolveProbe[it,:N//2] = ((angles+np.pi)%(2*np.pi)-qzero)
# wrap angles within -pi and pi
# not arctan(tan()) as it returns within -pi/2 and pi/2
rateEvolveProbe[it,:N//2] = (angles-qzero) # don't wrap angles, limit them or use trials so they don't run away
# subtract out the start position
rateEvolveProbe[it,N//2:] = (velocities-dqzero)
# for it,t in enumerate(robtrange):
# if trialclamp and (t%Tperiod)>(Tperiod-Tclamp):
# # at the end of each trial, bring arm to start position
# q = np.copy(qzero) # must copy, else pointer is used
# dq = np.copy(dqzero)
# else:
#torquet = inpfn(t)[N//2:] # torque to dv/dt ([:N//2] has only zeros for dq/dt) -- for ff+rec
# torquet = inpfn(t) # torque to dv/dt -- for rec
# torqueArray[it,:] = torquet
# qdot,dqdot = evolveFns(q,dq,torquet)
# q += qdot*robotdt
# dq += dqdot*robotdt
if 'rr_' not in funcType:
set_robot_state(q,dq)
else:rateEvolveProbe = csv_data[['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']].values
else:
raise Exception('Choose robotType')
sys.exit(1)
with contextlib.closing(
# 'c' opens for read/write, creating if non-existent
shelve.open(robDataFileName+'_'+robotType+'.shelve', 'c', protocol=-1)
) as data_dict:
data_dict['robtrange'] = robtrange
data_dict['angles'] = rateEvolveProbe
data_dict['torque'] = torqueArray
return robtrange,rateEvolveProbe,evolveFns,armAngles
def joint_torque(cid, handle, val):
vrep.simxSetJointTargetVelocity(cid, handle, val*100, vrep_mode)
vrep.simxSetJointForce(cid, handle, abs(val), vrep_mode)
def setMotorVelocity(velocity, torque):
vrep.simxSetJointForce(__clientID,rotor, torque, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(__clientID, rotor, velocity, vrep.simx_opmode_blocking)
def act(
self, turn_discr, accel_discr
): # entradas discretas, que hay que pasar a salidas continuas y no normalizadas
# turn_discr: string con estas posibilidades: {"very_left", "left", "slight_left", "front", "slight_right", "right", "very_right"}
# accel_discr: string con estas posibilidades: {"hard_break", "medium_break", "slight_break", "inertia", "slight_accel", "medium_accel", "full_gas"}
turn_dict = {
"[1, 0, 0, 0, 0, 0, 0]": 0, # "very_left"
"[0, 1, 0, 0, 0, 0, 0]": 0.2, # "left"
"[0, 0, 1, 0, 0, 0, 0]": 0.35, # "slight_left"
"[0, 0, 0, 1, 0, 0, 0]": 0.5, # "front"
"[0, 0, 0, 0, 1, 0, 0]": 0.65, # "slight_right"
"[0, 0, 0, 0, 0, 1, 0]": 0.8, # "right"
"[0, 0, 0, 0, 0, 0, 1]": 1 # "very_right"
}
steer_denorm = self.denorm(turn_dict[str(turn_discr)],
-self.max_steer_angle, self.max_steer_angle)
vrep.simxSetJointTargetPosition(self.clientID, self.steer_handle,
steer_denorm,
vrep.simx_opmode_streaming)
# accel_dict = { # (brake_force,motor_torque,motor_velocity)
# "[1, 0, 0, 0, 0, 0, 0]": (1, 0, 0), #"hard_break"
# "[0, 1, 0, 0, 0, 0, 0]": (0.5, 0, 0), #"medium_break"
# "[0, 0, 1, 0, 0, 0, 0]": (0 , 0, 0), #inertia
# "[0, 0, 0, 1, 0, 0, 0]": (0, 1, 0.25), #slight_accel
# "[0, 0, 0, 0, 1, 0, 0]": (0, 1, 0.5), #medium_accel
# "[0, 0, 0, 0, 0, 1, 0]": (0, 1, 0.75), #high_accel
# "[0, 0, 0, 0, 0, 0, 1]": (0, 1, 1) #"full_gas"
# }
accel_dict = { # (brake_force,motor_torque,motor_velocity)
"[1, 0, 0, 0, 0, 0, 0]": (0, 1, 0), #"hard_break"
"[0, 1, 0, 0, 0, 0, 0]": (0, 1, 0.17), #"medium_break"
"[0, 0, 1, 0, 0, 0, 0]": (0, 1, 0.33), #inertia
"[0, 0, 0, 1, 0, 0, 0]": (0, 1, 0.5), #slight_accel
"[0, 0, 0, 0, 1, 0, 0]": (0, 1, 0.67), #medium_accel
"[0, 0, 0, 0, 0, 1, 0]": (0, 1, 0.83), #high_accel
"[0, 0, 0, 0, 0, 0, 1]": (0, 1, 1) #"full_gas"
}
brake_force_norm, motor_torque_norm, motor_velocity_norm = accel_dict[
str(accel_discr)]
brake_force = self.denorm(brake_force_norm, self.min_brake,
self.max_brake)
motor_torque = self.denorm(motor_torque_norm, self.min_torque,
self.max_torque)
motor_velocity = self.denorm(motor_velocity_norm, self.min_accel,
self.max_accel)
vrep.simxSetJointForce(self.clientID, self.motor_handle, motor_torque,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.motor_handle,
motor_velocity,
vrep.simx_opmode_streaming)
vrep.simxSetJointForce(self.clientID, self.fr_brake_handle,
brake_force, vrep.simx_opmode_streaming)
vrep.simxSetJointForce(self.clientID, self.fl_brake_handle,
brake_force, vrep.simx_opmode_streaming)
vrep.simxSetJointForce(self.clientID, self.bl_brake_handle,
brake_force, vrep.simx_opmode_streaming)
vrep.simxSetJointForce(self.clientID, self.br_brake_handle,
brake_force, vrep.simx_opmode_streaming)
#
# velocity[ii] *= -1
#
# _ = vrep.simxSetJointTargetVelocity(
# clientID,
# joint_handle,
# velocity[ii],
# vrep.simx_opmode_blocking)
# if _ != 0:
# raise Exception('Error setting joint target velocity, ' +
# 'return code ', _)
# and now modulate the force
_ = vrep.simxSetJointForce(
clientID,
joint_handle,
uu[ii], # force to apply
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception(
'Error setting max joint force, ' + 'return code ', _)
# For a required number of time trigger the next simulation step
# for t in range(loop[k]):
vrep.simxSynchronousTrigger(clientID)
# vrep.simxGetPingTime(clientID)
k += 1
# time.sleep(1.0)
else:
raise Exception('Failed connecting to remote API server')
def joint_2_force():
vrep.simxSetJointForce(clientID, joint_2, 0.01, vrep.simx_opmode_oneshot)