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)