def coord2move_inv(pz, px, ph, clientID, joint1, joint2, joint3):
# Longitud links
l1 = 90
l2 = 66
l3 = 40
phi = np.deg2rad(ph)
wx = px - l3 * np.cos(phi)
wy = pz - l3 * np.sin(phi)
delta = wx**2 + wy**2
c2 = (delta - l1**2 - l2**2) / (2 * l1 * l2)
s2 = np.sqrt(1 - c2**2)
theta_2 = np.arctan2(float(s2), float(c2))
s1 = ((l1 + l2 * c2) * wy - l2 * s2 * wx) / delta
c1 = ((l1 + l2 * c2) * wx + l2 * s2 * wy) / delta
theta_1 = np.arctan2(float(s1), float(c1))
theta_1 = np.deg2rad(-np.rad2deg(theta_1))
theta_2 = np.deg2rad(-np.rad2deg(theta_2))
theta_3 = -phi - theta_1 - theta_2
# print('theta_1: ', rad2deg(theta_1))
# print('theta_2: ', rad2deg(theta_2))
# print('theta_3: ', rad2deg(theta_3))
_ = sim.simxSetJointTargetPosition(clientID, joint1, theta_1,
sim.simx_opmode_oneshot)
_ = sim.simxSetJointTargetPosition(clientID, joint2, theta_2,
sim.simx_opmode_oneshot)
_ = sim.simxSetJointTargetPosition(clientID, joint3, theta_3,
sim.simx_opmode_oneshot)
def coord2move(pz, px, ph, clientID, joint1, joint2, joint3):
l1 = 90
l2 = 66
l3 = 40
phi = np.deg2rad(ph)
wx = px - l3 * np.cos(phi)
wy = pz - l3 * np.sin(phi)
delta = wx**2 + wy**2
c2 = (delta - l1**2 - l2**2) / (2 * l1 * l2)
s2 = np.sqrt(1 - c2**2)
a = np.arctan2(float(s2), float(c2))
theta_2 = a
s1 = ((l1 + l2 * c2) * wy - l2 * s2 * wx) / delta
c1 = ((l1 + l2 * c2) * wx + l2 * s2 * wy) / delta
theta_1 = np.arctan2(float(s1), float(c1))
theta_3 = phi - theta_1 - theta_2
_ = sim.simxSetJointTargetPosition(clientID, joint1, theta_1,
sim.simx_opmode_oneshot)
_ = sim.simxSetJointTargetPosition(clientID, joint2, theta_2,
sim.simx_opmode_oneshot)
_ = sim.simxSetJointTargetPosition(clientID, joint3, theta_3,
sim.simx_opmode_oneshot)
def trasicion_atras_alante(clientID, joint1, joint2):
j = -88
for i in reversed(range(45, 114)):
j += 2
_ = sim.simxSetJointTargetPosition(clientID, joint2, np.deg2rad(j),
sim.simx_opmode_oneshot)
_ = sim.simxSetJointTargetPosition(clientID, joint1, np.deg2rad(i),
sim.simx_opmode_oneshot)
time.sleep(0.02)
def moverPaginaIzquierda(clientID, joint):
_ = sim.simxSetJointTargetVelocity(clientID, joint, 0.1,
sim.simx_opmode_oneshot)
_ = sim.simxSetJointTargetPosition(clientID, joint, np.deg2rad(0),
sim.simx_opmode_oneshot)