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 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 activate_magnet(teste):
status = 'active' if teste else 'deactive'
print("Magnet is {}".format(status))
try:
err_code, arm_actuator = sim.simxGetObjectHandle(
client.id, "suctionPad", sim.simx_opmode_oneshot)
if err_code != 0:
raise Exception("teste")
sim.simxAddStatusbarMessage(
client.id, 'Crab position request is: {}'.format(status),
sim.simx_opmode_oneshot)
err_code = sim.simxSetUserParameter(client.id, arm_actuator, status,
sim.simx_opmode_streaming)
except Exception:
print("Code error is {}".format(err_code))
def move_arm(rotation):
radians = (2 * math.pi * float(rotation)) / 100
print("degrees are {}".format(str((360 * radians) / (2 * math.pi))))
try:
err_code, arm_actuator = sim.simxGetObjectHandle(
client.id, "armActuator", sim.simx_opmode_blocking)
if err_code != 0:
raise Exception("teste")
sim.simxAddStatusbarMessage(
client.id, 'Command angle rotation send: {}'.format(str(radians)),
sim.simx_opmode_oneshot)
err_code = sim.simxSetJointTargetPosition(client.id, arm_actuator,
radians,
sim.simx_opmode_streaming)
except Exception:
print("Code error is {}".format(err_code))
def move_hoist(height):
final_height = (-0.68 * float(height)) / 100
print("height is {}".format(str(final_height)))
try:
err_code, arm_actuator = sim.simxGetObjectHandle(
client.id, "UpperMass", sim.simx_opmode_blocking)
if err_code != 0:
raise Exception("teste")
sim.simxAddStatusbarMessage(
client.id,
'Command angle rotation send: {}'.format(str(final_height)),
sim.simx_opmode_oneshot)
err_code = sim.simxSetJointTargetPosition(client.id, arm_actuator,
final_height,
sim.simx_opmode_streaming)
except Exception:
print("Code error is {}".format(err_code))
def move_crab(position):
final_position = (0.61 * float(position)) / 100
print("Position is {}".format(str(final_position)))
try:
err_code, arm_actuator = sim.simxGetObjectHandle(
client.id, "CrabMove", sim.simx_opmode_blocking)
if err_code != 0:
raise Exception("teste")
sim.simxAddStatusbarMessage(
client.id,
'Crab position request is: {}'.format(str(final_position)),
sim.simx_opmode_oneshot)
err_code = sim.simxSetJointTargetPosition(client.id, arm_actuator,
final_position,
sim.simx_opmode_streaming)
except Exception:
print("Code error is {}".format(err_code))
def movement():
sim.simxAddStatusbarMessage(clientID, 'Connected and running',
sim.simx_opmode_oneshot)
vel = 10
print("Forward")
sim.simxSetJointTargetVelocity(clientID, left_motor_handle, -vel,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID, right_motor_handle, -vel,
sim.simx_opmode_streaming)
time.sleep(40)
print("Left")
sim.simxSetJointTargetVelocity(clientID, left_motor_handle, vel,
sim.simx_opmode_streaming)
time.sleep(10)
sim.simxSetJointTargetVelocity(clientID, left_motor_handle, 0,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID, right_motor_handle, 0,
sim.simx_opmode_streaming)
print("Stop")
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)
import random
import time
import keyboard
import math
#from winsound import Beep
print('Start')
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
res = vrep.simxAddStatusbarMessage(clientID, "test-25",
vrep.simx_opmode_oneshot)
if res not in (vrep.simx_return_ok, vrep.simx_return_novalue_flag):
print("Could not add a message to the status bar.")
opmode = vrep.simx_opmode_oneshot_wait
STREAMING = vrep.simx_opmode_streaming
vrep.simxStartSimulation(clientID, opmode)
ret, joint1 = vrep.simxGetObjectHandle(clientID, "Revolute3_1", opmode)
ret, joint2 = vrep.simxGetObjectHandle(clientID, "Revolute3_2", opmode)
ret, joint3 = vrep.simxGetObjectHandle(clientID, "Revolute3_3", opmode)
ret, joint4 = vrep.simxGetObjectHandle(clientID, "Revolute3_4", opmode)
ret, joint5 = vrep.simxGetObjectHandle(clientID, "Revolute3_5", opmode)
ret, joint6 = vrep.simxGetObjectHandle(clientID, "Revolute3_6", opmode)
ret, joint01 = vrep.simxGetObjectHandle(clientID, "joint0_1", opmode)
sim.simx_opmode_blocking)
#print('returncode3=',return_code)
return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint4, theta[3],
sim.simx_opmode_blocking)
#print('returncode4=',return_code)
return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint5, theta[4],
sim.simx_opmode_blocking)
#print('returncode5=',return_code)
return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint6, theta[5],
sim.simx_opmode_blocking)
#print('returncode6=',return_code)
#sim.simxPauseCommunication(clientID,False)
if (FvI == 'F' or FvI == 'f'):
sim.simxSetObjectPosition(clientID, POI, -1, XYZ, sim.simx_opmode_blocking)
sim.simxAddStatusbarMessage(clientID, 'End of Code', sim.simx_opmode_oneshot)
print('End of Code')
res = input('reset? y/n:')
#print(res)
if (res == 'y' or res == 1):
return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint1, 0,
sim.simx_opmode_blocking)
return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint2, 0,
sim.simx_opmode_blocking)
return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint3, 0,
sim.simx_opmode_blocking)
return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint4, 0,
sim.simx_opmode_blocking)
return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint5, 0,
sim.simx_opmode_blocking)
return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint6, 0,
def detect_object(sensorHolder):
return_value, detectionState, detectionPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
clientID, leftSensorFront, sim.simx_opmode_buffer)
print(detectionPoint)
print(detectionState)
print('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19990, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID != -1:
print('Connected to remote API server')
# Now send some data to CoppeliaSim in a non-blocking fashion:
sim.simxAddStatusbarMessage(clientID, 'Hello from python!',
sim.simx_opmode_oneshot)
return_value, leftSensorFront = sim.simxGetObjectHandle(
clientID, 'Proximity_sensor_front_left', sim.simx_opmode_blocking)
return_value, leftSensorBack = sim.simxGetObjectHandle(
clientID, 'Proximity_sensor_back_left', sim.simx_opmode_blocking)
return_value, rightSensorFront = sim.simxGetObjectHandle(
clientID, 'Proximity_sensor_front_right', sim.simx_opmode_blocking)
return_value, rightSensorBack = sim.simxGetObjectHandle(
clientID, 'Proximity_sensor_back_right', sim.simx_opmode_blocking)
return_value, detectionState, detectionPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
clientID, leftSensorFront, sim.simx_opmode_streaming)
while (1):
detect_object(leftSensorFront)
import random
import time
import keyboard
import math
#from winsound import Beep
print('Start')
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
res = vrep.simxAddStatusbarMessage(clientID, "control_1",
vrep.simx_opmode_oneshot)
if res not in (vrep.simx_return_ok, vrep.simx_return_novalue_flag):
print("Could not add a message to the status bar.")
opmode = vrep.simx_opmode_oneshot_wait
STREAMING = vrep.simx_opmode_streaming
vrep.simxStartSimulation(clientID, opmode)
ret, joint01 = vrep.simxGetObjectHandle(clientID, "joint1", opmode)
ret, joint02 = vrep.simxGetObjectHandle(clientID, "joint2", opmode)
ret, joint03 = vrep.simxGetObjectHandle(clientID, "joint3", opmode)
ret, joint04 = vrep.simxGetObjectHandle(clientID, "joint4", opmode)
ret, joint05 = vrep.simxGetObjectHandle(clientID, "joint5", opmode)
ret, joint06 = vrep.simxGetObjectHandle(clientID, "joint6", opmode)
degset01 = 0
degset02 = 0
def show_msg(message):
""" send a message for printing in V-REP """
sim.simxAddStatusbarMessage(clientID, message, WAIT)
return
import sim as vrep
import math
import random
import time
import math
def moving(x,y):
a=0.4
b=0.4
c=math.pow((math.pow(x,2)+math.pow(y,2)),0.5)
s=(a+b+c)/2
area=math.pow((s*(s-a)*(s-b)*(s-c)),0.5)
h=area/(2*c)
deg1_base=math.atan(x/y)
if x<0 and y<0 :
deg_base1=deg1_base+math.pi
deg1_tri=math.asin(h/a)
deg1=deg1_base+deg1_tri
deg2=math.pi-(0.5*math.pi-deg1_tri)-math.acos(h/b)
deg3=deg2-deg1
vrep.simxSetJointTargetPosition(clientID,joint01,deg1,opmode)
vrep.simxSetJointTargetPosition(clientID,joint02,-deg2,opmode)
vrep.simxSetJointTargetPosition(clientID,joint03,deg3,opmode)
print ('Start')
vrep.simxFinish(-1)
clientID = vrep.simxStart('192.168.0.10', 19997, True, True, 5000, 5)
if clientID != -1:
print ('Connected to remote API server')
blockLocalDelivery,
myDirection)
entregar_cubo_colorido(cube)
##########################################
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)
robotname = 'Robot'
#targetname = 'Target1'
if clientID != -1:
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
print('Connected to remote API server')
sim.simxAddStatusbarMessage(clientID, 'Funcionando...',
sim.simx_opmode_oneshot_wait)
time.sleep(0.02)
# Coletar handles
#Robô
erro, robot = sim.simxGetObjectHandle(clientID, robotname,
sim.simx_opmode_blocking)
erro, robo = sim.simxGetObjectHandle(clientID, 'S_Base',
sim.simx_opmode_blocking)
#Motores
[erro,
robotLeftMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint2',
sim.simx_opmode_blocking)
[erro,
robotRightMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint1',
sim.simx_opmode_blocking)
print('Program started')
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server!')
res, v0 = vrep.simxGetObjectHandle(clientID, 'Vision_sensor',
vrep.simx_opmode_oneshot_wait)
res, resolution, imag = vrep.simxGetVisionSensorImage(
clientID, v0, 0, vrep.simx_opmode_streaming)
imcount = 0
while (vrep.simxGetConnectionId(clientID) != -1):
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, v0, 0, vrep.simx_opmode_buffer)
if res == vrep.simx_return_ok:
imcount = imcount + 1
res, rgb_resolution, rgb_image = vrep.simxGetVisionSensorImage(
clientID, v0, 0, vrep.simx_opmode_buffer)
rgb_img = np.array(rgb_image, dtype=np.uint8)
rgb_img.resize([rgb_resolution[1], rgb_resolution[0], 3])
rgb_img = cv2.flip(rgb_img, 0)
cv2.imshow("RGB_Image", rgb_img)
vrep.simxAddStatusbarMessage(clientID, 'hello world',
vrep.simx_opmode_oneshot)
cv2.waitKey(1)
print(imcount)
else:
print('Failed to show rgb and depth image')
else:
print('Failed to connect to vrep API server!')
print('Program ended!')
-0.2, -0.4, -0.6, -0.8, -1, -1.2, -1.4, -1.6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0
]
braitenbergR = [
-1.6, -1.4, -1.2, -1, -0.8, -0.6, -0.4, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0
]
v0 = 2
# Client Connection
clientID = sim.simxStart(serverIP, serverPort, True, True, timeOut, 5)
if clientID != -1:
# Now send some data to CoppeliaSim in a non-blocking fashion:
print('Connected to remote API server!')
sim.simxAddStatusbarMessage(clientID, 'Connected to remote API client!',
sim.simx_opmode_oneshot)
# Motors Initialization (remoteApi)
ret, pioneer.leftMotor.Handle = sim.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_leftMotor', sim.simx_opmode_oneshot_wait)
if ret != 0:
print("'Pioneer_p3dx_leftMotor' not found!")
else:
print("Linked to the 'Pioneer_p3dx_leftMotor' objHandle!")
ret, pioneer.rightMotor.Handle = sim.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_rightMotor', sim.simx_opmode_oneshot_wait)
if ret != 0:
print("'Pioneer_p3dx_rightMotor' not found!")
else:
def init(_clientID, robotname):
global robot, robo, robotLeftMotor, robotRightMotor
global clientID
global paEsquerda, paDireita
global elevador, irRight, irLeft
global color_sensor_Left, color_sensor_Right
global positionrobot, angLeft, angRight, orientationrobot
clientID = _clientID
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
print('Connected to remote API server')
sim.simxAddStatusbarMessage(clientID, 'Funcionando...',
sim.simx_opmode_oneshot_wait)
time.sleep(0.02)
# Coletar handles
#Robô
erro, robot = sim.simxGetObjectHandle(clientID, robotname,
sim.simx_opmode_blocking)
erro, robo = sim.simxGetObjectHandle(clientID, 'S_Base',
sim.simx_opmode_blocking)
#Motores
[erro,
robotLeftMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint2',
sim.simx_opmode_blocking)
[erro,
robotRightMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint1',
sim.simx_opmode_blocking)
#Garra
erro, paEsquerda = sim.simxGetObjectHandle(clientID,
'joint_pa_esquerda_garra',
sim.simx_opmode_blocking)
erro, paDireita = sim.simxGetObjectHandle(clientID,
'joint_pa_direita_garra',
sim.simx_opmode_blocking)
erro, elevador = sim.simxGetObjectHandle(clientID, 'joint_acoplador_garra',
sim.simx_opmode_blocking)
#Sensores
erro, irRight = sim.simxGetObjectHandle(clientID, 'Sensor_IR_direito',
sim.simx_opmode_blocking)
erro, irLeft = sim.simxGetObjectHandle(clientID, 'Sensor_IR_esquerdo',
sim.simx_opmode_blocking)
erro, color_sensor_Left = sim.simxGetObjectHandle(clientID,
'Sensor_cor_esquerda',
sim.simx_opmode_blocking)
erro, color_sensor_Right = sim.simxGetObjectHandle(
clientID, 'Sensor_cor_direita', sim.simx_opmode_blocking)
# Criar stream de dados
[erro,
positionrobot] = sim.simxGetObjectPosition(clientID, robot, -1,
sim.simx_opmode_streaming)
erro, angLeft = sim.simxGetJointPosition(clientID, robotLeftMotor,
sim.simx_opmode_streaming)
erro, angRight = sim.simxGetJointPosition(clientID, robotRightMotor,
sim.simx_opmode_streaming)
[erro, orientationrobot
] = sim.simxGetObjectOrientation(clientID, robot, -1,
sim.simx_opmode_streaming)
sim.simxReadProximitySensor(clientID, irRight, sim.simx_opmode_streaming)
sim.simxReadProximitySensor(clientID, irLeft, sim.simx_opmode_streaming)
sim.simxGetVisionSensorImage(clientID, color_sensor_Left, 0,
sim.simx_opmode_streaming)
sim.simxGetVisionSensorImage(clientID, color_sensor_Right, 0,
sim.simx_opmode_streaming)
