def read_FS():
global gripper_handles
penalty = 0
returnCode, state, forceVector0, torqueVector0 = vrep.simxReadForceSensor(
clientID, FS_handles[0], vrep.simx_opmode_buffer)
returnCode, state, forceVector1, torqueVector1 = vrep.simxReadForceSensor(
clientID, FS_handles[1], vrep.simx_opmode_buffer)
returnCode, state, forceVector2, torqueVector2 = vrep.simxReadForceSensor(
clientID, FS_handles[2], vrep.simx_opmode_buffer)
# print forceVector0, torqueVector0
# print forceVector1, torqueVector1
# print forceVector2, torqueVector2
force0 = (forceVector0[0]**2 + forceVector0[1]**2 +
forceVector0[2]**2)**(0.5)
force1 = (forceVector1[0]**2 + forceVector1[1]**2 +
forceVector1[2]**2)**(0.5)
force2 = (forceVector2[0]**2 + forceVector2[1]**2 +
forceVector2[2]**2)**(0.5)
# print force0, force1, force2
av_force = np.mean([force0, force1, force2])
force_dist = (abs(force0 - force1) + abs(force1 - force2) +
abs(force2 - force0)) / 3
force_dist_cost = 3 * (1 - np.exp(force_dist - 3))
if force_dist_cost < -20:
force_dist_cost = -20
print(av_force, force_dist, force_dist_cost)
returnCode, hand_joint_0 = vrep.simxGetJointPosition(
clientID, gripper_handles[2], vrep.simx_opmode_buffer)
returnCode, hand_joint_1 = vrep.simxGetJointPosition(
clientID, gripper_handles[3], vrep.simx_opmode_buffer)
returnCode, hand_joint_2 = vrep.simxGetJointPosition(
clientID, gripper_handles[4], vrep.simx_opmode_buffer)
if hand_joint_0 > 100 * math.pi / 180 or hand_joint_0 < 40 * math.pi / 180:
penalty = penalty + 10
if hand_joint_1 > 100 * math.pi / 180 or hand_joint_1 < 40 * math.pi / 180:
penalty = penalty + 10
if hand_joint_2 > 100 * math.pi / 180 or hand_joint_2 < 40 * math.pi / 180:
penalty = penalty + 10
return [av_force, force_dist_cost, penalty]
def _GetSensorValues(clientID, right, left, handles):
rightSensors = []
leftSensors = []
for senR, senL in zip(right, left):
errorCode, trash, values, trash2 = vrep.simxReadForceSensor(
clientID, handles[senR], vrep.simx_opmode_streaming)
rightSensors.append(values[2])
errorCode, trash, values, trash2 = vrep.simxReadForceSensor(
clientID, handles[senL], vrep.simx_opmode_streaming)
leftSensors.append(values[2])
return rightSensors, leftSensors
def obj_read_force_sensor(self, handle): state, forceVector, torqueVector = self.RAPI_rc(vrep.simxReadForceSensor( self.cID,handle, self.opM_get)) if state & 1 != 1: # bit 0 not set return None # sensor data not (yet) available elif state & 2 == 1: # bit 1 set return 0 # force sensor is broken else: return forceVector, torqueVector
def read(self) -> (bool, Vec3, Vec3):
"""
Reads the force and torque applied to a force sensor
(filtered values are read), and its current state ('unbroken' or 'broken').
"""
code, state, force, torque, snv = v.simxReadForceSensor(
self._id, self._handle, self._def_op_mode)
force_vector = Vec3(force[0], force[1], force[2])
torque_vector = Vec3(torque[0], torque[1], torque[2])
return state, force_vector, torque_vector
def readForceSensors(self):
for sensor in self.forceSensors.keys():
error, state, forceVector, torqueVector = vrep.simxReadForceSensor(
self.clientID, self.forceSensors[sensor],
vrep.simx_opmode_blocking)
time = vrep.simxGetLastCmdTime(clientID)
if error == 0:
self.XYZdata(sensor + 'force', forceVector, time)
self.XYZdata(sensor + 'torque', torqueVector, time)
else:
print("Force sensor read error: %d", error)
def get_accel_data(clientID, accelHandle, accelMassHandle):
# Retreive accelerometer signals:
res, state, force, torque = vrep.simxReadForceSensor(
clientID, accelHandle, vrep.simx_opmode_buffer)
accel = []
if res == vrep.simx_return_ok:
accel = [force[0] / mass, force[1] / mass, force[2] / mass]
return res, accel
def read_sensor_values(simx_opmode=vrep.simx_opmode_streaming):
'''read force sensor values of the left and right foot'''
lfsr_values = [None] * len(LFsrNames)
rfsr_values = [None] * len(RFsrNames)
for index, name in enumerate(LFsrNames):
if _LFsrHandles[index]:
returnCode, state, forces, torques = vrep.simxReadForceSensor(
clientID, _LFsrHandles[index], simx_opmode)
if returnCode:
print("Failed reading force sensor %s (error: %d)" % (
name, returnCode))
else:
lfsr_values[index] = forces[2]
for index, name in enumerate(RFsrNames):
if _RFsrHandles[index]:
returnCode, state, forces, torques = vrep.simxReadForceSensor(
clientID, _RFsrHandles[index], vrep.simx_opmode_streaming)
if returnCode:
print("Failed reading force sensor %s (error code: %d)" % (
name, returnCode))
else:
rfsr_values[index] = forces[2]
return lfsr_values, rfsr_values
def recover(n=N):
force = np.zeros((6, 3))
torque = np.zeros((6, 3))
Lz = np.zeros(n)
init_position = np.zeros((6, 3))
for i in range(6):
res, init_position[i] = vrep.simxGetObjectPosition(clientID, S1[i], BCS, vrep.simx_opmode_oneshot_wait)
for i in range(n):
Lz[i] = init_position[0][2] - i * 0.18/n
ret, F_state, force[0], torque[0] = vrep.simxReadForceSensor(clientID, FS[0], vrep.simx_opmode_streaming)
print("state1:", F_state)
print("ForceArray:", force[0])
print("TorqueArray:", torque[0])
for i in range(n):
for j in range(0,6,2):
vrep.simxSynchronousTrigger(clientID)
vrep.simxSetObjectPosition(clientID, Tip_target[j], BCS, [init_position[j][0], init_position[j][1], Lz[i]],
vrep.simx_opmode_oneshot_wait)
ret, F_state, force[0], torque[0] = vrep.simxReadForceSensor(clientID, FS[0], vrep.simx_opmode_streaming)
print("state2:", F_state)
print("ForceArray:", force[0])
print("TorqueArray:", torque[0])
for i in range(n):
for j in range(1,6,2):
vrep.simxSynchronousTrigger(clientID)
vrep.simxSetObjectPosition(clientID, Tip_target[j], BCS, [init_position[j][0], init_position[j][1], Lz[i]],
vrep.simx_opmode_oneshot_wait)
ret, F_state, force[0], torque[0] = vrep.simxReadForceSensor(clientID, FS[0], vrep.simx_opmode_streaming)
print("state3:", F_state)
print("ForceArray:", force[0])
print("TorqueArray:", torque[0])
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')
else:
sys.exit('Failed connecting to remote API server')
#wheel joints
errorCode, force_sensor_handle = vrep.simxGetObjectHandle(
clientID, 'Force_sensor', vrep.simx_opmode_blocking)
print("Force_sensor", errorCode, force_sensor_handle)
errorCode, forceState, forceVector, torqueVector = vrep.simxReadForceSensor(
clientID, force_sensor_handle, vrep.simx_opmode_streaming)
print("init force sensor", errorCode, forceState, forceVector, torqueVector)
FBvalue = 0
RLvalue = 0
rotvalue = 0
joint1 = 300
joint2 = 450
joint3 = 130
joint4 = 140
joint5 = 500
timer = 0
while (1):
errorCode, forceState, forceVector, torqueVector = vrep.simxReadForceSensor(
clientID, force_sensor_handle, vrep.simx_opmode_buffer)
timer += 1
def step(self, action):
done = False #round indicator
action = np.clip(action, *self.action_bound) #get action form network
#set FK or IK
vrep.simxSetIntegerSignal(self.clientID, "movementMode",
self.movementMode, vrep.simx_opmode_oneshot)
#read force sensor
self.errorCode, self.forceState, self.forceVector, self.torqueVector = vrep.simxReadForceSensor(
self.clientID, self.force_sensor_handle, vrep.simx_opmode_buffer)
#calculations
#
#
#
#
#take actions
if (self.movementMode): #in IK mode
#do action
self.IK['Pos_x'] += action[0]
self.IK['Pos_y'] += action[1]
self.IK['Pos_z'] += action[2]
self.IK['Alpha'] += action[3]
self.IK['Beta'] += action[4]
self.IK['Gamma'] += action[5]
#send signal
vrep.simxSetFloatSignal(self.clientID, "pos_X", self.IK['Pos_x'],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self.clientID, "pos_Y", self.IK['Pos_y'],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self.clientID, "pos_Z", self.IK['Pos_z'],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self.clientID, "Alpha", self.IK['Alpha'],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self.clientID, "Beta", self.IK['Beta'],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self.clientID, "Gamma", self.IK['Gamma'],
vrep.simx_opmode_oneshot)
time.sleep(0.1) #wait for action to finish
else: #in FK mode
#do action
self.FK['Joint1'] += action[0]
self.FK['Joint2'] += action[1]
self.FK['Joint3'] += action[2]
self.FK['Joint4'] += action[3]
self.FK['Joint5'] += action[4]
self.FK['Joint6'] += action[5]
#boundary
self.FK['Joint1'] = np.clip(self.FK['Joint1'],
*self.Joint_boundary[0])
self.FK['Joint2'] = np.clip(self.FK['Joint2'],
*self.Joint_boundary[1])
self.FK['Joint3'] = np.clip(self.FK['Joint3'],
*self.Joint_boundary[2])
self.FK['Joint4'] = np.clip(self.FK['Joint4'],
*self.Joint_boundary[3])
self.FK['Joint5'] = np.clip(self.FK['Joint5'],
*self.Joint_boundary[4])
self.FK['Joint6'] = np.clip(self.FK['Joint6'],
*self.Joint_boundary[5])
#send signal
vrep.simxSetFloatSignal(
self.clientID, "Joint1",
(self.FK['Joint1'] * np.pi / 180 - self.Joints[0][0]) /
self.Joints[0][1] * 1000, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(
self.clientID, "Joint2",
(self.FK['Joint2'] * np.pi / 180 - self.Joints[1][0]) /
self.Joints[1][1] * 1000, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(
self.clientID, "Joint3",
(self.FK['Joint3'] * np.pi / 180 - self.Joints[2][0]) /
self.Joints[2][1] * 1000, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(
self.clientID, "Joint4",
(self.FK['Joint4'] * np.pi / 180 - self.Joints[3][0]) /
self.Joints[3][1] * 1000, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(
self.clientID, "Joint5",
(self.FK['Joint5'] * np.pi / 180 - self.Joints[4][0]) /
self.Joints[4][1] * 1000, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(
self.clientID, "Joint6",
(self.FK['Joint6'] * np.pi / 180 - self.Joints[5][0]) /
self.Joints[5][1] * 1000, vrep.simx_opmode_oneshot)
time.sleep(0.1) #wait for action to finish
# done and reward
r = 0
#
#
#
#
# state
s = [1, 2, 3, 4, 5, 6, 7, 8, 9]
#
#
#
#
return s, r, done
def __init__(self):
#vrep init session
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to V-REP, get clientID
if self.clientID != -1: #confirm connection
print('Connected to remote API server')
else:
sys.exit('Failed connecting to remote API server')
vrep.simxSetIntegerSignal(self.clientID, "Apimode", 1,
vrep.simx_opmode_oneshot) #activate apimode
#vrep sensor setp
#Setup the force sensor
self.errorCode, self.force_sensor_handle = vrep.simxGetObjectHandle(
self.clientID, 'IRB140_connection', vrep.simx_opmode_blocking)
print("IRB140_connection", self.errorCode, self.force_sensor_handle)
self.errorCode, self.forceState, self.forceVector, self.torqueVector = vrep.simxReadForceSensor(
self.clientID, self.force_sensor_handle,
vrep.simx_opmode_streaming)
print("init force sensor IRB140_connection", self.errorCode,
self.forceState, self.forceVector, self.torqueVector)
#Get Joint data
self.Joints = np.zeros((6, 2))
self.Joint_boundary = np.zeros((6, 2))
for i in range(6):
for j in range(2):
self.errorCode, self.Joints[i][j] = vrep.simxGetFloatSignal(
self.clientID, 'Interval_{}_{}'.format(i + 1, j + 1),
vrep.simx_opmode_streaming)
while (self.errorCode):
self.errorCode, self.Joints[i][
j] = vrep.simxGetFloatSignal(
self.clientID,
'Interval_{}_{}'.format(i + 1, j + 1),
vrep.simx_opmode_buffer)
#print(self.errorCode,'Interval_{}_{}'.format(i+1,j+1),self.Joints[i][j])
self.Joint_boundary[i] = [(self.Joints[i][0] / np.pi * 180),
((self.Joints[i][0] / np.pi * 180) +
(self.Joints[i][1] / np.pi * 180))]
print(self.Joint_boundary[i])
#Setup controllable variables
self.movementMode = 0 #work under FK(0) or IK(1)
self.FK = np.zeros(1,
dtype=[
('Joint1', np.float32), ('Joint2', np.float32),
('Joint3', np.float32), ('Joint4', np.float32),
('Joint5', np.float32), ('Joint6', np.float32)
])
#initial angle in FK mode
self.FK['Joint1'] = 0
self.FK['Joint2'] = 0
self.FK['Joint3'] = 0
self.FK['Joint4'] = 0
self.FK['Joint5'] = -90
self.FK['Joint6'] = 0
self.IK = np.zeros(1,
dtype=[('Pos_x', np.float32), ('Pos_y', np.float32),
('Pos_z', np.float32), ('Alpha', np.float32),
('Beta', np.float32), ('Gamma', np.float32)])
#initial value in IK mode
self.IK['Pos_x'] = 0
self.IK['Pos_y'] = 0
self.IK['Pos_z'] = 0
self.IK['Alpha'] = 0
self.IK['Beta'] = 0
self.IK['Gamma'] = 0
# a. Accelerometers:
res1, accelHandle = vrep.simxGetObjectHandle(clientID,
"Accelerometer_forceSensor",
vrep.simx_opmode_blocking)
res2, accelMassHandle = vrep.simxGetObjectHandle(clientID,
"Accelerometer_mass",
vrep.simx_opmode_blocking)
# Get the mass of the accelerometer parent:
res3, mass = vrep.simxGetObjectFloatParameter(clientID, accelMassHandle,
vrep.sim_shapefloatparam_mass,
vrep.simx_opmode_oneshot_wait)
# Stream the data the first time:
res, state, force, torque = vrep.simxReadForceSensor(
clientID, accelHandle, vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
print(
" --- Error getting accelerometer data for the first time ( Can be ignored!)--- "
)
if res1 != vrep.simx_return_ok and res2 != vrep.simx_return_ok and res3 != vrep.simx_return_ok:
print(" --- There was an error getting the Accelerometer Handle ---")
# b. Gyroscopes:
res, gyroHandle = vrep.simxGetObjectHandle(clientID, "GyroSensor",
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
def associate_handlers():
'''get handlers involved joints and sensors'''
global _PositionHandle, _OrientationHandle
for index, name in enumerate(JointNames):
returnCode, _JointHandles[index] = vrep.simxGetObjectHandle(
clientID, name + '#', vrep.simx_opmode_blocking)
if returnCode:
print("Failed to associate with %s (error: %d)" % (
name, returnCode))
else: # make sure joint handlers work properly
returnCode, position = vrep.simxGetJointPosition(
clientID, _JointHandles[index],
vrep.simx_opmode_streaming)
for index, name in enumerate(LHandJointNames):
returnCode, _LHandJointHandles[index] = vrep.simxGetObjectHandle(
clientID, name + '#', vrep.simx_opmode_blocking)
if returnCode:
print("Failed to associate with %s (error: %d)" % (
name, returnCode))
else: # make sure left hand finger handlers work properly
returnCode, position = vrep.simxGetJointPosition(
clientID, _LHandJointHandles[index],
vrep.simx_opmode_streaming)
for index, name in enumerate(RHandJointNames):
returnCode, _RHandJointHandles[index] = vrep.simxGetObjectHandle(
clientID, name + '#', vrep.simx_opmode_blocking)
if returnCode:
print("Failed to associate with %s (error: %d)" % (
name, returnCode))
else: # make sure right hand finger handlers work properly
returnCode, position = vrep.simxGetJointPosition(
clientID, _RHandJointHandles[index],
vrep.simx_opmode_streaming)
for index, name in enumerate(LFsrNames):
returnCode, _LFsrHandles[index] = vrep.simxGetObjectHandle(
clientID, name + '#', vrep.simx_opmode_blocking)
if returnCode:
print("Failed to associate with %s (error: %d)" % (
name, returnCode))
else: # make sure LFsr handlers work properly
returnCode, state, forces, torques = vrep.simxReadForceSensor(
clientID, _LFsrHandles[index], vrep.simx_opmode_streaming)
for index, name in enumerate(RFsrNames):
returnCode, _RFsrHandles[index] = vrep.simxGetObjectHandle(
clientID, name + '#', vrep.simx_opmode_blocking)
if returnCode:
print("Failed to associate with %s (error: %d)" % (
name, returnCode))
else: # make sure RFsr handlers work properly
returnCode, state, forces, torques = vrep.simxReadForceSensor(
clientID, _RFsrHandles[index], vrep.simx_opmode_streaming)
returnCode, _PositionHandle = vrep.simxGetObjectHandle(
clientID, PositionName + '#', vrep.simx_opmode_blocking)
if returnCode:
print("Failed to associate with %s (error: %d)" % (
PositionName, returnCode))
else: # make sure position handler works properly
returnCode, position = vrep.simxGetObjectPosition(
clientID, _PositionHandle, -1,
vrep.simx_opmode_streaming)
returnCode, _OrientationHandle = vrep.simxGetObjectHandle(
clientID, OrientationName + '#', vrep.simx_opmode_blocking)
if returnCode:
print("Failed to associate with %s (error: %d)" % (
OrientationName, returnCode))
else: # make sure orientation handler works properly
returnCode, orientation = vrep.simxGetObjectOrientation(
clientID, _OrientationHandle, -1,
vrep.simx_opmode_streaming)
for index, name in enumerate(VisionNames):
returnCode, _VisionHandles[index] = vrep.simxGetObjectHandle(
clientID, name + '#', vrep.simx_opmode_blocking)
if returnCode:
print("Failed to associate with %s (error: %d)" % (
name, returnCode))
else: # make sure vision handlers work properly
returnCode, resolution, image = vrep.simxGetVisionSensorImage(
clientID, _VisionHandles[index], 0,
vrep.simx_opmode_streaming)
time.sleep(0.1)
