def __init__(self): # n_actions:3 (target pos), n_states:6 (3pos+3force)
self.metadata = {'render.modes': ['human']}
super().__init__()
sim.simxFinish(-1)
for _ in range(5):
self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000,
5)
if self.clientID != -1:
# print('[INFO] Connected to CoppeliaSim.')
break
if self.clientID == -1:
raise IOError('[ERROR] Could not connect to CoppeliaSim.')
sim.simxSynchronous(self.clientID, True)
# sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
self.stepCount = 0
self.reward = 0
self.n_substeps = 10
# self.sim_timestep = 0.5 # set in coppeliaSim (not implemented)
self.n_actions = 3
self.n_states = 6
self.action_space = spaces.Box(-1.,
1,
shape=(self.n_actions, ),
dtype='float32')
self.observation_space = spaces.Box(-np.inf,
np.inf,
shape=(self.n_states, ),
dtype='float32')
self._getHandles()
sim.simxGetPingTime(self.clientID)
def close(self):
sim.simxAddStatusbarMessage(self.clientID, 'Goodbye CoppeliaSim!',
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot)
time.sleep(2)
sim.simxFinish(self.clientID)
def exit_remote_api_server():
"""
Purpose:
---
This function should wait for the last command sent to arrive at the Coppeliasim server
before closing the connection and then end the communication thread with server
i.e. CoppeliaSim using simxFinish Remote API.
Input Arguments:
---
None
Returns:
---
None
Example call:
---
exit_remote_api_server()
NOTE: This function will be automatically called by test_task_2a executable after ending the simulation.
"""
global client_id
############## ADD YOUR CODE HERE ##############
sim.simxGetPingTime(client_id)
sim.simxFinish(client_id)
def close_connection(self):
sim.simxGetPingTime(
self.client_id
) # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive.
sim.simxFinish(
self.client_id) # Now close the connection to CoppeliaSim:
print('Connection closed')
def close_connection(self):
self.actuate([0]*len(self.motors))
# Before closing the connection of CoppeliaSim,
# make sure that the last command sent out had time to arrive.
sim.simxGetPingTime(self.client_id)
sim.simxFinish(self.client_id) # Now close the connection of CoppeliaSim:
print('Connection closed')
def start_Simulation(self):
print('Program started')
# Just in case, close all opened connections
sim.simxFinish(-1)
# Connect to CoppeliaSim, as continuous remote API Server
self.clientID = sim.simxStart('127.0.0.1', 19997, False, True, 5000, 5)
if self.clientID != -1:
print('Connected to remote API server')
#Start simulation
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
# Now send some data to CoppeliaSim in a non-blocking fashion:
sim.simxAddStatusbarMessage(self.clientID, 'Connection Succeed!',
sim.simx_opmode_oneshot)
# Before closing the connection to CoppeliaSim, make sure that the last command sent
# out had time to arrive. You can guarantee this with (for example):
sim.simxGetPingTime(self.clientID)
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = sim.simxGetObjects(self.clientID, sim.sim_handle_all,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
print('Number of objects in the scene: ', len(objs))
else:
print('Remote API function call returned with error code: ',
res)
time.sleep(2)
else:
print('Failed connecting to remote API server')
return self.clientID
def reset(self):
sim.simxSetFloatSignal(self.clientID, 'renderEnv', 0,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
self.resetSim()
self.prepareSim()
obs = self._get_obs()
return obs
def stop_simulation(self):
# Now send some data to CoppeliaSim in a non-blocking fashion:
sim.simxAddStatusbarMessage(self.clientID, 'Quit',
sim.simx_opmode_oneshot)
# Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
sim.simxGetPingTime(self.clientID)
# Now close the connection to CoppeliaSim:
sim.simxFinish(self.clientID)
def disconnect_from_server(self):
# We wait for all the conveyor timers to expire
while self.current_thread is not None:
# We skip the execution of this loop
pass
# Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive
sim.simxGetPingTime(self.client_id)
# Now close the connection to CoppeliaSim
sim.simxFinish(self.client_id)
# Helps store the client id allocated by the coppeliasim server
self.client_id = -1
def getHandle(self, name, ignoreError=False, attempts=5):
for _ in range(attempts):
res, out_handle = sim.simxGetObjectHandle(self.clientID, name,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok or ignoreError:
# print('[INFO] {} handle obtained.'.format(name))
break
sim.simxGetPingTime(self.clientID)
# if res!=sim.simx_return_ok and not ignoreError:
# print('[WARNING] Failed to find {} with error {}.'.format(name, res))
return out_handle
def _set_action(self, action):
actionX = action[0]
actionY = action[1]
actionZ = action[2]
# print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ))
sim.simxSetFloatSignal(self.clientID, 'actionX', actionX,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionY', actionY,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
def stop_Simulation(self):
#Stop Run Simulation
sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot)
# Before closing the connection to CoppeliaSim, make sure that the last
# command sent out had time to arrive. You can guarantee this with (for example):
sim.simxGetPingTime(self.clientID)
# Now close the connection to CoppeliaSim:
sim.simxFinish(self.clientID)
print('Program ended')
return None
def resetSim(self):
# print('[INFO] reseting sim...')
ret = sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
if ret == sim.simx_return_ok:
# print('[INFO] sim reset successfully.')
time.sleep(1)
else:
raise IOError('[ERROR] problem in reseting sim...')
sim.simxGetPingTime(self.clientID)
# print('[INFO] starting sim in synchronous mode...')
sim.simxSynchronous(self.clientID, True)
sim.simxGetPingTime(self.clientID)
ret = sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
# if ret == sim.simx_return_ok:
# print('[INFO] sim started successfully.')
sim.simxGetPingTime(self.clientID)
sim.simxClearFloatSignal(self.clientID, 'actionX',
sim.simx_opmode_blocking)
sim.simxClearFloatSignal(self.clientID, 'actionY',
sim.simx_opmode_blocking)
sim.simxClearFloatSignal(self.clientID, 'actionZ',
sim.simx_opmode_blocking)
sim.simxGetPingTime(self.clientID)
self.stepCount = 0
self.reward = 0
def getForce(self, initialize=True):
if initialize:
F1 = sim.simxReadForceSensor(self.clientID, self.fsHandles[0],
sim.simx_opmode_streaming)[2]
F2 = sim.simxReadForceSensor(self.clientID, self.fsHandles[1],
sim.simx_opmode_streaming)[2]
F3 = sim.simxReadForceSensor(self.clientID, self.fsHandles[2],
sim.simx_opmode_streaming)[2]
else:
F1 = sim.simxReadForceSensor(self.clientID, self.fsHandles[0],
sim.simx_opmode_buffer)[2]
F2 = sim.simxReadForceSensor(self.clientID, self.fsHandles[1],
sim.simx_opmode_buffer)[2]
F3 = sim.simxReadForceSensor(self.clientID, self.fsHandles[2],
sim.simx_opmode_buffer)[2]
sim.simxGetPingTime(self.clientID)
return [F1, F2, F3]
def getKinectXYZ(self, initialize=True):
if initialize:
X = sim.simxGetFloatSignal(self.clientID, 'actX',
sim.simx_opmode_streaming)[1]
Y = sim.simxGetFloatSignal(self.clientID, 'actY',
sim.simx_opmode_streaming)[1]
Z = sim.simxGetFloatSignal(self.clientID, 'actZ',
sim.simx_opmode_streaming)[1]
else:
X = sim.simxGetFloatSignal(self.clientID, 'actX',
sim.simx_opmode_buffer)[1]
Y = sim.simxGetFloatSignal(self.clientID, 'actY',
sim.simx_opmode_buffer)[1]
Z = sim.simxGetFloatSignal(self.clientID, 'actZ',
sim.simx_opmode_buffer)[1]
sim.simxGetPingTime(self.clientID)
return X, Y, Z
def getForceMagnitude(self, initialize=True):
if initialize:
F1 = sim.simxReadForceSensor(self.clientID, self.fsHandles[0],
sim.simx_opmode_streaming)[2]
F2 = sim.simxReadForceSensor(self.clientID, self.fsHandles[1],
sim.simx_opmode_streaming)[2]
F3 = sim.simxReadForceSensor(self.clientID, self.fsHandles[2],
sim.simx_opmode_streaming)[2]
else:
F1 = sim.simxReadForceSensor(self.clientID, self.fsHandles[0],
sim.simx_opmode_buffer)[2]
F2 = sim.simxReadForceSensor(self.clientID, self.fsHandles[1],
sim.simx_opmode_buffer)[2]
F3 = sim.simxReadForceSensor(self.clientID, self.fsHandles[2],
sim.simx_opmode_buffer)[2]
sim.simxGetPingTime(self.clientID)
return [np.linalg.norm(F1), np.linalg.norm(F2), np.linalg.norm(F3)]
def doTracking(self):
self.resetSim()
self.prepareSim()
pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone',
sim.simx_opmode_streaming)[1]
posX = []
posY = []
posZ = []
tactile1 = []
reward = []
while pathIsDone == 0:
X, Y, Z = self.getKinectXYZ(False)
# forces = self.getForce(False)
# diffForce = (forces[0][0] + ((forces[1][0] + forces[2][0]) / 2))/2
# print('[DATA] diff force: {:.4f}'.format(diffForce))
actionX = X # + 0.005*diffForce
actionY = Y
actionZ = Z
# print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ))
sim.simxSetFloatSignal(self.clientID, 'actionX', actionX,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionY', actionY,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
sim.simxSynchronousTrigger(self.clientID)
forces = self.getForceMagnitude(False)
posX.append(X)
posY.append(Y)
posZ.append(Z)
tactile1.append(forces[0])
reward.append(self._getReward())
sim.simxGetPingTime(self.clientID)
pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone',
sim.simx_opmode_buffer)[1]
# time.sleep(0.5)
# print('[INFO] tracking is done.')
# print('[DATA] accumulated reward: {}'.format(np.sum(np.array(reward))))
sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
def full_sim_reset(self):
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking)
is_running = True
while is_running:
error_code, ping_time = vrep.simxGetPingTime(self.clientID)
error_code, server_state = vrep.simxGetInMessageInfo(
self.clientID, vrep.simx_headeroffset_server_state)
if server_state == 0:
is_running = False
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
time.sleep(1)
def prepareSim(self):
self.initializeFunctions()
isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking',
sim.simx_opmode_streaming)[1]
while isTracking != 1:
sim.simxSynchronousTrigger(self.clientID)
isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking',
sim.simx_opmode_buffer)[1]
sim.simxGetPingTime(self.clientID)
X, Y, Z = self.getKinectXYZ(True)
actionX = X
actionY = Y
actionZ = Z
# print(actionX,actionY,actionZ)
# print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ))
sim.simxSetFloatSignal(self.clientID, 'actionX', actionX,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionY', actionY,
sim.simx_opmode_oneshot)
sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ,
sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
def transfer_genomes(self, timeout, project):
"""
transfer the genome files to the evo-client
"""
ret, ping_time = sim.simxGetPingTime(self.clientID)
checksum = 0
sim.simxClearIntegerSignal(self.clientID, "ClientID_Signal", mode1)
sim.simxSetIntegerSignal(self.clientID, "ClientID_Signal",
self.clientID, mode1)
for i, filename in enumerate(self.genome_filenames):
file_to_transfer = project.path_to_gene_pool + filename
ret = sim.simxTransferFile(self.clientID, file_to_transfer,
filename, timeout,
mode1) # default: mode2
if ret == 0:
checksum += 1
return checksum
def main():
np.set_printoptions(precision=3, suppress=True) # Restrict decimals in console output
dof = 6
home_position, screw = ur5_params() # Get UR5 parameters
theta_0 = np.array([0, 0, 0, 0, 0, 0]) # Initial position
theta_d = np.array([0,-m.pi/2, 0, 0, m.pi/2, 0]) # Desired position, converted to x_d in the solver
T_0 = compute_T(screw, theta_0, dof, home_position)
T_d = compute_T(screw, theta_d, dof, home_position)
print("Home position: \n", T_0, "\n")
print("Desired position: \n", T_d, "\n")
# Find solution to the system
theta, delta, n = root_finding(theta_0, theta_d, 20, dof, home_position, screw)
T_theta = compute_T(screw, theta, dof, home_position)
print('Solution : \n', theta, '\n', 'Transformation : \n', T_theta, '\n')
R = T_theta[0:3][0:3] # Get RPY for the solution
r_roll = m.atan2(R[2][1],R[2][2])
r_pitch = m.atan2(-R[2][0],m.sqrt(R[2][2]*R[2][2] + R[2][1]*R[2][1]))
r_yaw = m.atan2(R[1][0],R[0][0])
# Begin connection with coppeliaSim.
# Robot simulation must be running in coppeliaSim to connect
sim.simxFinish(-1) # just in case, close all opened connections
clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim
# clientID stores the ID assingned by coppeliaSim, if the connection failed it will be assigned -1
if clientID!=-1:
print ('Connected to remote API server')
# Get ID handles for each joint of the robot
res,UR5_joint1 =sim.simxGetObjectHandle( clientID, 'UR5_joint1', sim.simx_opmode_blocking)
res,UR5_joint2 =sim.simxGetObjectHandle( clientID, 'UR5_joint2', sim.simx_opmode_blocking)
res,UR5_joint3 =sim.simxGetObjectHandle( clientID, 'UR5_joint3', sim.simx_opmode_blocking)
res,UR5_joint4 =sim.simxGetObjectHandle( clientID, 'UR5_joint4', sim.simx_opmode_blocking)
res,UR5_joint5 =sim.simxGetObjectHandle( clientID, 'UR5_joint5', sim.simx_opmode_blocking)
res,UR5_joint6 =sim.simxGetObjectHandle( clientID, 'UR5_joint6', sim.simx_opmode_blocking)
res,UR5_endEffector =sim.simxGetObjectHandle( clientID, 'UR5_connection', sim.simx_opmode_blocking)
# Store the handles as a list
UR5 = [UR5_joint1, UR5_joint2, UR5_joint3, UR5_joint4, UR5_joint5, UR5_joint6] # Just grab the first 3 for now to test
# Get current coordinates of each joint
UR5_q = []
for joint in UR5:
res, value = sim.simxGetJointPosition(clientID, joint, sim.simx_opmode_oneshot)
UR5_q.append(value) # Store current values
# Set new coordinates for the robot in coppeliaSim
# Add time delays so the animation can be displayed correctly
steps = 100
position_desired = theta
for t in range(steps):
i = 0
k = 0
for joint in UR5:
sim.simxSetJointTargetPosition(clientID, joint, t*(position_desired[i])/steps, sim.simx_opmode_streaming)
res, value = sim.simxGetJointPosition(clientID, joint, sim.simx_opmode_oneshot)
if t == 0 or t == 99:
k += 1
res, position = sim.simxGetObjectPosition(clientID, UR5_endEffector, UR5_joint1, sim.simx_opmode_oneshot)
res, orientation = sim.simxGetObjectOrientation(clientID, UR5_endEffector, UR5_joint1, sim.simx_opmode_oneshot)
i += 1
time.sleep(2/steps)
# Convert robot angles to the 4x4 matrix representation for comparison
c1, s1 = np.cos(orientation[0]), np.sin(orientation[0])
c2, s2 = np.cos(orientation[1]), np.sin(orientation[1])
c3, s3 = np.cos(orientation[2]), np.sin(orientation[2])
R_ur5 = np.block([
[c2 * c3, -c2 * s3, s2],
[c1 * s3 + c3 * s1 * s2, c1 * c3 - s1 * s2 * s3, -c2 * s1],
[s1 * s3 - c1 * c3 * s2, c3 * s1 + c1 * s2 * s3, c1 * c2]
])
p_ur5 = np.array(position).reshape((3,1))
T_ur5 = np.block([
[R_ur5, p_ur5],
[0, 0, 0, 1]
])
print('\n Robot coordinates: \n ', T_ur5 )
# print('\n Absolute Error : \n', abs(T_theta - T_ur5))
time.sleep(1)
# print('\n Returning to home position...')
# for joint in UR5:
# sim.simxSetJointTargetPosition(clientID, joint, 0, sim.simx_opmode_streaming)
# Now send some data to CoppeliaSim in a non-blocking fashion:
sim.simxAddStatusbarMessage(clientID,'Excuting forward kinematics in Python',sim.simx_opmode_oneshot)
# Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
sim.simxGetPingTime(clientID)
# Now close the connection to CoppeliaSim:
sim.simxFinish(clientID)
def isConnected(self):
c, result = sim.simxGetPingTime(self.client_id)
# Return true if the robot is connected
return result > 0
def main():
global hit_wall
global ball_coord
global ball_linear
global ball_hit
global detect_handle
global prox_handle
global stop_prog
print('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID != -1:
print('Connected to remote API server')
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
print('Number of objects in the scene: ', len(objs))
else:
print('Remote API function call returned with error code: ', res)
time.sleep(2)
#Giving an initial velocity to the ball before starting the thread
detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere',
sim.simx_opmode_blocking)
prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor',
sim.simx_opmode_blocking)
racket_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor0',
sim.simx_opmode_blocking)
dummy_handle = sim.simxGetObjectHandle(clientID, 'Cylinder',
sim.simx_opmode_blocking)
sim.simxSetObjectPosition(clientID, detect_handle[1], -1,
[0.65, 0.47, 0.0463],
sim.simx_opmode_oneshot)
sim.simxPauseCommunication(clientID, True)
sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3001, 1,
sim.simx_opmode_oneshot)
#sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3000, -0.01, sim.simx_opmode_oneshot)
sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3002, 1,
sim.simx_opmode_oneshot)
sim.simxPauseCommunication(clientID, False)
sim.simxReadProximitySensor(clientID, prox_handle[1],
sim.simx_opmode_streaming)
sim.simxGetObjectVelocity(clientID, detect_handle[1],
sim.simx_opmode_streaming)
sim.simxReadProximitySensor(clientID, racket_handle[1],
sim.simx_opmode_streaming)
sim.simxGetObjectPosition(clientID, dummy_handle[1], -1,
sim.simx_opmode_streaming)
ball_thread = threading.Thread(target=get_ball_info,
args=({
clientID: clientID
}))
try:
#getting joint handles and initializing the joints in the simulation
print("1. getting joint angles")
jointHandles = []
for i in range(6):
handle = sim.simxGetObjectHandle(
clientID, 'UR3_joint' + str(i + 1) + '#0',
sim.simx_opmode_blocking)
jointHandles.append(handle)
time.sleep(0.01)
for i in range(6):
print(jointHandles[i])
#hit_thread = threading.Thread(target=hit_ball, args=({clientID:clientID}))
ball_thread.daemon = True
#hit_thread.daemon = True
#hit_thread.start()
ball_thread.start()
# #Get handles for detecting object
# detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking)
# #Get handles for detecting the proximity sensor
# prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor', sim.simx_opmode_blocking)
#Create an instance to compute the ball trajectory
print("1. Initializing bouncing trajectory function")
b_ball = bouncing_ball.BouncingBall()
#Set-up some of the RML vectors:
vel = 60
accel = 10
jerk = 20
currentVel = [0, 0, 0, 0, 0, 0, 0]
currentAccel = [0, 0, 0, 0, 0, 0, 0]
maxVel = [
vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180,
vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180
]
maxAccel = [
accel * math.pi / 180, accel * math.pi / 180,
accel * math.pi / 180, accel * math.pi / 180,
accel * math.pi / 180, accel * math.pi / 180
]
maxJerk = [
jerk * math.pi / 180, jerk * math.pi / 180,
jerk * math.pi / 180, jerk * math.pi / 180,
jerk * math.pi / 180, jerk * math.pi / 180
]
targetVel = [0, 0, 0, 0, 0, 0]
sim.simxPauseCommunication(clientID, True)
for i in range(6):
sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], 0,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1],
targetVel[i],
sim.simx_opmode_streaming)
sim.simxPauseCommunication(clientID, False)
time.sleep(2)
#start the feedback loop
while True:
if not hit_wall:
#print("We haven't hit the wall yet")
continue
if hit_wall:
print("We hit wall ....")
#Predict the position of the ball from the wall and transform it to world coordinates
print("Predicting Trajectory ...")
pred_pos = b_ball.trajectory(ball_coord[0], ball_coord[1],
ball_coord[2], ball_linear)
T_prox = np.array([[-1, 0, 0, 0.025], [0, -1, 0, 2.85],
[0, 0, 1, -.05], [0, 0, 0, 1]])
prox_pos = np.array([[pred_pos[0]], [pred_pos[1]],
[pred_pos[2]], [1]])
ball_pos = T_prox @ prox_pos
print(ball_pos)
#set the left flag to true if on left side
left = 0
if ball_pos[0, :] < 0: left = 1
#convert to right-side coordinates for IK
ball_pos[0, :] = np.abs(ball_pos[0, :])
if ball_pos[0, :] > 0.9:
print("Ball too far away from robot. will not hit it ...")
raise ValueError
elif ball_pos[0, :] < 0.2:
print("Ball too close. cannot hit it...")
raise ValueError
twist_angle = (-92.85) * (ball_pos[0, :]) + 90
print(ball_pos, "left?: ", left)
targetPos0_ik = ik.findJointAngles(ball_pos[0, :],
ball_pos[1, :],
ball_pos[2, :] + 0.15)
#Invert joint angles if on left side of robot
for i in range(len(targetPos0_ik)):
targetPos0_ik[i] = ((-2 * left) + 1) * targetPos0_ik[i]
print("Applying the hitting motion")
#Apply the hitting motion using the new joint angles
end_pose = []
targetPos0_ik[0] = targetPos0_ik[0] + (
(-2 * left) + 1) * (0 * np.pi / 180) #To simulate a hit
targetPos0_ik[4] = targetPos0_ik[4] + (
(-2 * left) + 1) * (0 * np.pi / 180)
sim.simxPauseCommunication(clientID, True)
for i in range(6):
#print(jointHandles[i])
#print(targetPos0[i])
sim.simxSetJointTargetPosition(clientID,
jointHandles[i][1],
targetPos0_ik[i],
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID,
jointHandles[i][1],
targetVel[i],
sim.simx_opmode_streaming)
sim.simxPauseCommunication(clientID, False)
#Now read from the proximity sensor to see if it detects any ball
ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor(
clientID, racket_handle[1], sim.simx_opmode_buffer)
while (dS == 0):
_, end_pose = sim.simxGetObjectPosition(
clientID, dummy_handle[1], -1, sim.simx_opmode_buffer)
ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor(
clientID, racket_handle[1], sim.simx_opmode_buffer)
print("Status of Ball is: ", dS)
#Now, attach a proximity sensor to the racquet and see if it detects a ball. If it does, let's start the
#hitting motion
#Actually, for now, let's just not worry about this. Let's make sure that the trajectory generation and the ball hitting
#works
print("Twist angle is: ", twist_angle)
targetPos0_ik[0] = targetPos0_ik[0] + ((-2 * left) + 1) * (
(1 * twist_angle) * np.pi / 180) #To simulate a hit
targetPos0_ik[4] = targetPos0_ik[4] + (
(-2 * left) + 1) * (-1 * twist_angle * np.pi / 180)
# sim.simxPauseCommunication(clientID, True)
# for i in range(6):
# #print(jointHandles[i])
# #print(targetPos0[i])
# sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], targetPos0_ik[i], sim.simx_opmode_buffer)
# sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_buffer)
# sim.simxPauseCommunication(clientID, False)
# time.sleep(2)
ball_hit = True
hit_wall = False
sim.simxPauseCommunication(clientID, True)
for i in [0, 4]:
sim.simxSetJointTargetPosition(clientID,
jointHandles[i][1],
targetPos0_ik[i],
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID,
jointHandles[i][1],
targetVel[i],
sim.simx_opmode_streaming)
sim.simxPauseCommunication(clientID, False)
time.sleep(2)
sim.simxPauseCommunication(clientID, True)
#Reset after hitting ball
for i in range(6):
#print(jointHandles[i])
#print(targetPos0[i])
sim.simxSetJointTargetPosition(clientID,
jointHandles[i][1], 0,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID,
jointHandles[i][1],
targetVel[i],
sim.simx_opmode_streaming)
sim.simxPauseCommunication(clientID, False)
##Inverse Kinematics experiment and analysis: CAN BE COMMENTED OUT
ball_pos[0, :] = (-2 * left + 1) * ball_pos[0, :]
ball_pos[2, :] = ball_pos[2, :] + 0.15
end_pose = np.array(end_pose).reshape((3, 1))
print("Desired position: \n", ball_pos[:3])
print("End-effector position: \n", end_pose[:3])
# print("Error (MMSE): ", np.linalg.norm(ball_pos[:3] - end_pose[:3]))
# x_err = np.abs((end_pose[0,:] - ball_pos[0,:])/(ball_pos[0,:])) * 100
# y_err = np.abs((end_pose[1,:] - ball_pos[1,:])/(ball_pos[1,:])) * 100
# z_err = np.abs((end_pose[2,:] - ball_pos[2,:])/(ball_pos[2,:])) * 100
# print("Error in X (%): ", x_err)
# print("Error in Y (%): ", y_err)
# print("Error in Z (%): ", z_err)
# print("Total Error (%): ", (x_err + y_err + z_err)/3)
except:
print(sys.exc_info())
#hit_thread.join()
stop_prog = True
print("Exception encountered, stopping program")
#ball_thread.join()
print("Ball thread stopped")
sim.simxGetPingTime(clientID)
# Now close the connection to CoppeliaSim:
sim.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
sim.simxGetPingTime(clientID)
# Now close the connection to CoppeliaSim:
sim.simxFinish(clientID)
#control_force = 400
if np.sign(control_force) >= 0:
set_force = control_force
set_velocity = 9999
else:
set_force = -control_force
set_velocity = -9999
# 控制命令需要同时方式，故暂停通信，用于存储所有控制命令一起发送
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetVelocity(clientID, jointHandle, set_velocity,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID, jointHandle, set_force,
vrep.simx_opmode_oneshot)
#vrep.simxSetJointForce(clientID,jointHandle,set_force,vrep.simx_opmode_oneshot);
# vrep.simxSetJointTargetPosition(clientID, jointHandle, 100, vrep.simx_opmode_oneshot)
# vrep.simxSetJointTargetPosition(clientID, jointHandle[1], 5/RAD2DEG, vrep.simx_opmode_oneshot)
# vrep.simxSetJointTargetVelocity(clientID, jointHandle[2], 500/RAD2DEG, vrep.simx_opmode_oneshot)
# vrep.simxSetJointTargetVelocity(clientID, jointHandle[3], 500/RAD2DEG, vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
# ***
lastCmdTime = currCmdTime # 记录当前时间
vrep.simxSynchronousTrigger(clientID) # 进行下一步
vrep.simxGetPingTime(clientID) # 使得该仿真步走完
def _step_simulation(self):
"""Шаг симуляции для режима синхронизации
"""
sim.simxSynchronousTrigger(self.client)
sim.simxGetPingTime(self.client)
vrep.simxSetObjectPosition(clientID, objecthandle, -1, a,
vrep.simx_opmode_oneshot)
if keyboard.is_pressed("end"):
a[2] -= 0.0006
vrep.simxSetObjectPosition(clientID, objecthandle, -1, a,
vrep.simx_opmode_oneshot)
if keyboard.is_pressed("page up"):
rot[2] += 0.0004
vrep.simxSetObjectOrientation(clientID, objecthandle, -1, rot,
vrep.simx_opmode_oneshot)
if keyboard.is_pressed("page down"):
rot[2] -= 0.0004
vrep.simxSetObjectOrientation(clientID, objecthandle, -1, rot,
vrep.simx_opmode_oneshot)
if keyboard.is_pressed('q'):
break
#break # if user pressed a key other than the given key the loop will break
vrep.simxAddStatusbarMessage(clientID, 'Hello V-REP!',
vrep.simx_opmode_oneshot)
vrep.simxGetPingTime(clientID)
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
packedMovementData=msgpack.packb(movementData)
sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
targetConfig=[0,0,0,0,0,0]
targetVel=[0,0,0,0,0,0]
movementData={"id":"movSeq3","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel}
packedMovementData=msgpack.packb(movementData)
sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
# Execute second and third movement sequence:
sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot)
sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot)
# Wait until above 2 movement sequences finished executing:
waitForMovementExecuted1('movSeq3')
waitForMovementExecuted1('movSeq3')
sim.simxStopSimulation(client.id,sim.simx_opmode_blocking)
sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_discontinue)
sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_discontinue)
sim.simxGetPingTime(client.id)
# Now close the connection to CoppeliaSim:
sim.simxFinish(client.id)
else:
print ('Failed connecting to remote API server')
Sensor = sim.simxReadProximitySensor(clientID, sensor,
sim.simx_opmode_buffer)
print(Sensor[1])
return Sensor
def LineSensor(Sensor):
Vsensor = sim.simxReadVisionSensor(clientID, Sensor,
sim.simx_opmode_buffer)
return Vsensor[-1][0][1:4]
#Conectandose a Coppelia
clientID = Connect(19997)
Velocity = 100 * math.pi / 180
print(sim.simxGetPingTime(clientID))
#Handle de los joints
Sensor_Handle = Handle('Sensor')
Carro = Handle('Pioneer_p3dx')
MotorL = Handle('Left')
MotorR = Handle('Right')
Vsensor_L = Handle('Vsensor_L')
Vsensor_R = Handle('Vsensor_R')
#Inicializar sensores
sim.simxReadProximitySensor(clientID, Sensor_Handle, sim.simx_opmode_streaming)
sim.simxReadVisionSensor(clientID, Vsensor_L, sim.simx_opmode_streaming)
sim.simxReadVisionSensor(clientID, Vsensor_R, sim.simx_opmode_streaming)
sim.simxGetObjectPosition(clientID, Carro, -1, sim.simx_opmode_streaming)
# Set Robot Position
sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1,
sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2,
sim.simx_opmode_oneshot)
# Start the Simulation
print("Robot1 Position:", Loc1)
print("Robot2 Position:", Loc2)
print("Robot1 Orientation:", OriRobo1)
print("Robot2 Orientation:", OriRobo2)
print("Simulation Running ...")
#sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
time.sleep(1)
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
iter += 1
# Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
sim.simxGetPingTime(clientID)
# End Connection to V-Rep
sim.simxFinish(clientID)
else:
print("Failed connecting to remote API server")
print("Program Ended")
def main(argv):
# https://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm --> how to remote API
# Refer to https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxStart
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
print('Failed connecting to remote API server')
else:
print('Connected to remote API client')
numberOfRuns = 10
numberOfMeasurements = 1
variationSize = 0.001
for i in range(numberOfRuns):
count = 0
dt = 0.0167
sim.simxSynchronous(clientID, True)
################################################### init variables ###################################################
ros_handle = simROS.SIMROS(argv)
targetAngle = 0
#targetAngles = list(np.random.randint(-25, 26, numberOfMeasurements))
targetAngles = list(np.zeros(numberOfMeasurements))
print(targetAngles)
randomVar = []
for i in range(18):
randomVar.append(
list(
np.random.uniform(-variationSize, variationSize,
numberOfMeasurements)))
if newFile:
with open(dataFilePath, 'w+') as myfile:
wr = csv.writer(myfile, quoting=csv.QUOTE_ALL)
wr.writerow([
"Inclination", "LFx", "LFy", "LFz", "LMx", "LMy",
"LMz", "LHx", "LHy", "LHz", "RFx", "RFy", "RFz", "RMx",
"RMy", "RMz", "RHx", "RHy", "RHz"
])
counter = 0
Done = False
#Start the simulation
sim.simxStartSimulation(clientID, sim.simx_opmode_blocking)
################################################### Control Loop ###################################################
while not Done:
#code here
#print(sim.simxReadForceSensor(clientID, "3D_force0", sim.simx_opmode_blocking))
if counter == 150:
saveData(force, targetAngle)
print("Runs remaining: ", len(targetAngles))
if len(targetAngles) <= 0:
Done = True
else:
targetAngle = targetAngles.pop(0)
motor_positions = [
0, randomVar[1].pop() + 2.059, randomVar[2].pop(),
0, randomVar[4].pop() + 2.059, randomVar[5].pop(),
0, randomVar[7].pop() + 2.059, randomVar[8].pop(),
0, randomVar[10].pop() + 2.059,
randomVar[11].pop(), 0,
randomVar[13].pop() + 2.059, randomVar[14].pop(),
0, randomVar[16].pop() + 2.059,
randomVar[17].pop()
]
counter = 0
ros_handle.setSlopeAngle(targetAngle)
ros_handle.setLegMotorPosition(motor_positions)
counter = counter + 1
#Step the simulation
sim.simxSynchronousTrigger(clientID)
sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)
sim.simxGetPingTime(clientID)
sim.simxFinish(clientID)
