def UR3_inverse_kinematics(targetT, clientID, jointHandles): # x, y, z on vrep plane: V-REP has x, y, z markers on the bottom right of the screen # TODO: one contraint to ease would be to develop a method agnostic of rotation # above idea useful for end-effectors that dont rely on orientation as much, such as suction cups M = get_zeroconfig_M(clientID, jointHandles) S = get_zeroconfig_S(clientID, jointHandles) initThetas = [ 0, PI / 2, 0, 0, 0, 0 ] # initial thetas robot starts at (PI/2 because most objects are going to be on the ground) eomg = [0.2] ev = [0.2] # error parameters # IKinSpace() uses the Newton-Rahpson method [targetThetaList, IKsuccess] = mr.IKinSpace(S, M, targetT, initThetas, eomg, ev) if IKsuccess: set_joint_position(np.array(targetThetaList), clientID, jointHandles) # Reasons for not being succesful: either due to the method or due to physical constraints return IKsuccess, targetThetaList
def set_ee_pose_matrix(self, T_sd, custom_guess=None, execute=True, moving_time=None, accel_time=None, blocking=True): if (custom_guess is None): initial_guesses = self.initial_guesses else: initial_guesses = [custom_guess] for guess in initial_guesses: theta_list, success = mr.IKinSpace(self.robot_des.Slist, self.robot_des.M, T_sd, guess, 0.001, 0.001) solution_found = True # Check to make sure a solution was found and that no joint limits were violated if success: theta_list = [int(elem * 1000) / 1000.0 for elem in theta_list] for x in range(self.resp.num_joints): if not (self.resp.lower_joint_limits[x] <= theta_list[x] <= self.resp.upper_joint_limits[x]): solution_found = False break else: solution_found = False if solution_found: if execute: self.publish_positions(theta_list, moving_time, accel_time, blocking) self.T_sb = T_sd return theta_list, True else: rospy.loginfo("Guess failed to converge...") rospy.loginfo("No valid pose could be found") return theta_list, False
def move_to_high(self, thetalist0): # Solve IK self.thetalist, success = mr.IKinSpace(self.Slist, self.M, self.T, thetalist0, self.eomg, self.ev) self.IK_validation(self.thetalist) # Nove to the initial position self.waypoints['right_j0'] = self.thetalist[0] self.waypoints['right_j1'] = self.thetalist[1] self.waypoints['right_j2'] = self.thetalist[2] self.waypoints['right_j3'] = self.thetalist[3] self.waypoints['right_j4'] = self.thetalist[4] self.waypoints['right_j5'] = self.thetalist[5] self.waypoints['right_j6'] = 0.0 self.mylimb.move_to_joint_positions(self.waypoints, timeout=20.0, threshold=0.05) # Publish the command to perception node rospy.sleep(1) # (-1, 0, 0) for perceiving all four tags self.command.x = -1 self.point_pub.publish(self.command)
def plot_face_trajectory(self, limb, limb_interface): Slist, M, eomg, ev = self.get_parameters_for_IK() n_sec = 0.5 n_sec_for_hang = 5 z_touch = 0.160 z_hang = 0.210 x_cutoff = -0.20 y_cutoff = -0.17 T = np.array([[0, -1, 0, 0.575], [-1, 0, 0, 0.1603], [0, 0, -1, 0.317], [0, 0, 0, 1]]) #plot_points=self.plot_points i = 0 marker_hanup = True line_traj = Trajectory(limb, limb_interface.joint_names()) rospy.on_shutdown(line_traj.stop) #default step, add current_angles as initial thetalist0 current_angles = [ limb_interface.joint_angle(joint) for joint in limb_interface.joint_names() ] thetalist0 = current_angles line_traj.add_point(current_angles, 0.0) #start drawing position thetalist0 = [0, -np.pi / 3.0, 0, np.pi / 2, 0, np.pi / 6, 0] line_traj.add_point(thetalist0, n_sec_for_hang) plot_points = self.trajectory print("Solving IK") thetalist0 = [-0.66, -0.91, 0.04, 2.21, -0.11, 0.28, -0.45] while i < len(plot_points): if marker_hanup == True: #move right above the start point T[0][3] = plot_points[i][0] * 0.005 + self.target.x + x_cutoff T[1][3] = plot_points[i][1] * 0.005 + self.target.y + y_cutoff thetalist0, success = mr.IKinSpace(Slist, M, T, thetalist0, eomg, ev) # print("Bad IK Solution point: ", T[0][3], T[1][3], T[2][3], i,plot_points[i][0], plot_points[i][1]) # print("Joint angles: ", thetalist0) self.IK_validation(thetalist0) if i == 0: print("fisrt angles", thetalist0) line_traj.add_point(thetalist0, n_sec_for_hang) #drop down marker T[2][3] = self.target.z + z_touch thetalist0, success = mr.IKinSpace(Slist, M, T, thetalist0, eomg, ev) # print("Bad IK Solution point: ", T[0][3], T[1][3], T[2][3], i,plot_points[i][0], plot_points[i][1]) # print("Joint angles: ", thetalist0) self.IK_validation(thetalist0) line_traj.add_point(thetalist0, n_sec_for_hang) marker_hanup = False i += 1 else: if plot_points[i][0] == -1 and plot_points[i][1] == -1: # lift up the marker T[2][3] = self.target.z + z_hang thetalist0, success = mr.IKinSpace(Slist, M, T, thetalist0, eomg, ev) # print("Bad IK Solution point: ", T[0][3], T[1][3], T[2][3], i,plot_points[i][0], plot_points[i][1]) # print("Joint angles: ", thetalist0) self.IK_validation(thetalist0) line_traj.add_point(thetalist0, n_sec_for_hang) marker_hanup = True else: T[0][3] = plot_points[i][ 0] * 0.005 + self.target.x + x_cutoff T[1][3] = plot_points[i][ 1] * 0.005 + self.target.y + y_cutoff thetalist0, success = mr.IKinSpace(Slist, M, T, thetalist0, eomg, ev) # print("Bad IK Solution point: ", T[0][3], T[1][3], T[2][3], i,plot_points[i][0], plot_points[i][1]) # print("Joint angles: ", thetalist0) self.IK_validation(thetalist0) line_traj.add_point(thetalist0, n_sec) i += 1 print("Finishing Solving IK") #start to draw line_traj.start() line_traj.wait(line_traj.duration) print("Finish Drawing !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
def controller(self, event): # check end-effector related commands if (sum([ self.joy_msg.ee_x_cmd, self.joy_msg.ee_y_cmd, self.joy_msg.ee_z_cmd, self.joy_msg.ee_roll_cmd, self.joy_msg.ee_pitch_cmd ]) > 0 and self.follow_pose == False): # Copy the most recent T_yb transform into a temporary variable T_yb = np.array(self.T_yb) # check ee_x_cmd if (self.joy_msg.ee_x_cmd == ArmJoyControl.EE_X_INC): T_yb[0, 3] += 0.0025 * self.joy_speeds["current"] elif (self.joy_msg.ee_x_cmd == ArmJoyControl.EE_X_DEC): T_yb[0, 3] -= 0.0025 * self.joy_speeds["current"] # check ee_y_cmd if (self.joy_msg.ee_y_cmd == ArmJoyControl.EE_Y_INC and self.num_joints >= 6): T_yb[1, 3] += 0.0025 * self.joy_speeds["current"] elif (self.joy_msg.ee_y_cmd == ArmJoyControl.EE_Y_DEC and self.num_joints >= 6): T_yb[1, 3] -= 0.0025 * self.joy_speeds["current"] # check ee_z_cmd if (self.joy_msg.ee_z_cmd == ArmJoyControl.EE_Z_INC): T_yb[2, 3] += 0.0025 * self.joy_speeds["current"] elif (self.joy_msg.ee_z_cmd == ArmJoyControl.EE_Z_DEC): T_yb[2, 3] -= 0.0025 * self.joy_speeds["current"] # check end-effector orientation related commands if (self.joy_msg.ee_roll_cmd != 0 or self.joy_msg.ee_pitch_cmd != 0): rpy = ang.rotationMatrixToEulerAngles(T_yb[:3, :3]) # check ee_roll_cmd if (self.joy_msg.ee_roll_cmd == ArmJoyControl.EE_ROLL_CCW): rpy[0] += 0.01 * self.joy_speeds["current"] elif (self.joy_msg.ee_roll_cmd == ArmJoyControl.EE_ROLL_CW): rpy[0] -= 0.01 * self.joy_speeds["current"] # check ee_pitch_cmd if (self.joy_msg.ee_pitch_cmd == ArmJoyControl.EE_PITCH_DOWN): rpy[1] += 0.01 * self.joy_speeds["current"] elif (self.joy_msg.ee_pitch_cmd == ArmJoyControl.EE_PITCH_UP): rpy[1] -= 0.01 * self.joy_speeds["current"] T_yb[:3, :3] = ang.eulerAnglesToRotationMatrix(rpy) # Get desired transformation matrix of the end-effector w.r.t. the base frame T_sd = np.dot(self.T_sy, T_yb) theta_list, success = mr.IKinSpace(self.robot_des.Slist, self.robot_des.M, T_sd, self.joint_positions, 0.001, 0.001) joint_limits_violated = False # Check to make sure no joint limits were violated for x in range(self.num_joints): if not (self.resp.lower_joint_limits[x] <= theta_list[x] <= self.resp.upper_joint_limits[x]): joint_limits_violated = True break # Also check to make sure that the 'IKinSpace' function found a valid solution. # Otherwise, don't update self.joint_positions or the other transforms if (success and not joint_limits_violated): self.T_sb = T_sd self.T_yb = T_yb self.joint_positions = theta_list self.joint_commands.cmd = self.joint_positions self.pub_joint_commands.publish(self.joint_commands) else: rospy.loginfo("No valid pose could be found") # check waist_cmd if (self.joy_msg.waist_cmd != 0 and self.follow_pose == False): if (self.joy_msg.waist_cmd == ArmJoyControl.WAIST_CCW and self.joint_positions[self.joint_indx_dict["waist"]] < self.resp.upper_joint_limits[self.joint_indx_dict["waist"]] ): self.joint_positions[self.joint_indx_dict[ "waist"]] += 0.01 * self.joy_speeds["current"] self.yaw += 0.01 * self.joy_speeds["current"] if (self.joint_positions[self.joint_indx_dict["waist"]] > self.resp.upper_joint_limits[ self.joint_indx_dict["waist"]]): self.joint_positions[self.joint_indx_dict[ "waist"]] = self.resp.upper_joint_limits[ self.joint_indx_dict["waist"]] self.yaw = self.resp.upper_joint_limits[ self.joint_indx_dict["waist"]] elif (self.joy_msg.waist_cmd == ArmJoyControl.WAIST_CW and self.joint_positions[self.joint_indx_dict["waist"]] > self.resp.lower_joint_limits[self.joint_indx_dict["waist"]]): self.joint_positions[self.joint_indx_dict[ "waist"]] -= 0.01 * self.joy_speeds["current"] self.yaw -= 0.01 * self.joy_speeds["current"] if (self.joint_positions[self.joint_indx_dict["waist"]] < self.resp.lower_joint_limits[ self.joint_indx_dict["waist"]]): self.joint_positions[self.joint_indx_dict[ "waist"]] = self.resp.lower_joint_limits[ self.joint_indx_dict["waist"]] self.yaw = self.resp.lower_joint_limits[ self.joint_indx_dict["waist"]] self.T_sy[:2, :2] = self.yaw_to_rotation_matrix(self.yaw) self.T_sb = np.dot(self.T_sy, self.T_yb) self.joint_commands.cmd = self.joint_positions self.pub_joint_commands.publish(self.joint_commands) # get updated joint positions with self.js_mutex: js_msg = list(self.joint_states.position) # check gripper position if (self.gripper_moving): if ((self.gripper_command.data > 0 and js_msg[self.gripper_index] >= self.resp.upper_gripper_limit) or (self.gripper_command.data < 0 and js_msg[self.gripper_index] <= self.resp.lower_gripper_limit)): self.gripper_command.data = 0 self.pub_gripper_command.publish(self.gripper_command) self.gripper_moving = False # check if the arm should slowly move to the 'Home' or 'Sleep' pose if (self.follow_pose): poses_are_equal = True for x in range(self.num_joints): if (self.target_positions[x] > self.joint_positions[x]): self.joint_positions[ x] += 0.01 * self.joy_speeds["current"] if (self.joint_positions[x] > self.target_positions[x]): self.joint_positions[x] = self.target_positions[x] poses_are_equal = False elif (self.target_positions[x] < self.joint_positions[x]): self.joint_positions[ x] -= 0.01 * self.joy_speeds["current"] if (self.joint_positions[x] < self.target_positions[x]): self.joint_positions[x] = self.target_positions[x] poses_are_equal = False self.joint_commands.cmd = self.joint_positions self.pub_joint_commands.publish(self.joint_commands) if (poses_are_equal): self.follow_pose = False rospy.loginfo("Done following pose")
def pseudoInverse(input): x = input[0] y = input[1] J = np.array([[2 * x, 0], [0, 2 * y]]) pseudoInverse = np.linalg.inv(J) return pseudoInverse # Question 1 # f(x,y) = [x^2 - 9, y^2 - 4] # Initial Guess solution1 = np.array([1, 1]) goal = np.array([0, 0]) print solution1 for i in range(2): solution1 = solution1 + np.dot(pseudoInverse(solution1), (goal - forwardFunction(solution1))) print solution1 # Question 2 Slist = np.array([[0, 0, 0], [0, 0, 0], [1, 1, 1], [0, 0, 0], [0, -1, -2], [0, 0, 0]]) M = np.array([[1, 0, 0, 3], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) Tsd = np.array([[-0.585, -0.811, 0, 0.076], [0.811, -0.585, 0, 2.608], [0, 0, 1, 0], [0, 0, 0, 1]]) thetaList = np.array([math.pi / 4, math.pi / 4, math.pi / 4]) Sol = mr.IKinSpace(Slist, M, Tsd, thetaList, 0.001, 0.0001) print Sol
[0.50645468, -0.81103411, -0.29279229, -7.42921783], [0.13431932, 0.40962139, -0.90231294, 4.30158108], [0.00000000, 0.00000000, 0.00000000, 1.00000000]]) M = np.array([[0.00000000, 1.00000000, 0.00000000, 6.00000000], [1.00000000, 0.00000000, 0.00000000, -4.00000000], [0.00000000, 0.00000000, -1.00000000, 0.00000000], [0.00000000, 0.00000000, 0.00000000, 1.00000000]]) S = np.array([ [0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000], [-1.00000000, -1.00000000, 0.00000000, 0.00000000, 0.00000000, 1.00000000], [0.00000000, 0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000], [0.00000000, 0.00000000, 0.00000000, -6.00000000, 0.00000000, 0.00000000], [0.00000000, 0.00000000, 0.00000000, -2.00000000, 1.00000000, 0.00000000], [-2.00000000, 0.00000000, 6.00000000, 0.00000000, 0.00000000, 4.00000000] ]) print(mr.IKinSpace(S, M, T_1in0, [0, 0, 0, 0, 0, 0], 0.01, 0.01)) print("\n question 4 \n") s1 = np.array([0, 0, 1, 0, 2, 0]) #-2,0,0 s2 = np.array([0, 1, 0, 0, 0, -2]) #-2,2,0 s3 = np.array([0, 0, 1, 4, 2, 0]) # -2,4,0 s4 = np.array([0, 1, 0, 0, 0, -2]) #-2,6,0 s5 = np.array([0, -1, 0, 0, 0, 0]) # 0,6,0 s6 = np.array([0, 0, 0, 1, 0, 0]) #rotate x by +90 #rotate y by 90 T_1in0 = np.array([[-0.53170421, 0.40925994, -0.74148293, -2.33801009], [-0.42110581, 0.63185226, 0.65071701, 5.63880461],
import modern_robotics as mr import numpy as np T_1in0 = np.array([[0.91527031, -0.39524763, -0.07784322, -3.61453845], [0.38172744, 0.91269119, -0.14587308, -4.55327714], [0.12870281, 0.10379841, 0.98623602, 0.80724058], [0.00000000, 0.00000000, 0.00000000, 1.00000000]]) M = np.array([[1.00000000, 0.00000000, 0.00000000, -4.00000000], [0.00000000, 1.00000000, 0.00000000, -6.00000000], [0.00000000, 0.00000000, 1.00000000, 0.00000000], [0.00000000, 0.00000000, 0.00000000, 1.00000000]]) S = np.array( [[0.00000000, 0.00000000, 0.00000000, 0.00000000, 0.00000000, 0.00000000], [ 0.00000000, -1.00000000, -1.00000000, 0.00000000, -1.00000000, 0.00000000 ], [0.00000000, 0.00000000, 0.00000000, 0.00000000, 0.00000000, 0.00000000], [-1.00000000, 0.00000000, 0.00000000, 0.00000000, 0.00000000, 0.00000000], [0.00000000, 0.00000000, 0.00000000, 1.00000000, 0.00000000, 1.00000000], [0.00000000, 2.00000000, 2.00000000, 0.00000000, 6.00000000, 0.00000000]]) thetalist0 = np.array([0, 0, 0, 0, 0, 0]) eomg = 0.01 ev = 0.001 result = mr.IKinSpace(S, M, T_1in0, thetalist0, eomg, ev) print(result)
def solve(self): return mr.IKinSpace(self.Slist, self.M, self.T, self.theta0, self.eomg, self.ev)
#Desired Configuration: Ti = np.array([[-1,0,0,0], [0,1,0,200], [0,0,-1,0], [0,0,0,1]]) Tf = np.array([[-1,0,0,100], [0,1,0,200], [0,0,-1,300], [0,0,0,1]]) Fk = mr.FKinSpace(Robot.M,Robot.Slist.T,[np.deg2rad(90),np.deg2rad(0),np.deg2rad(90),np.deg2rad(90),np.deg2rad(90),np.deg2rad(0)]) Ik = mr.IKinSpace(Robot.Slist.T,Robot.M,Ti,BestTheta0(Ti[0][3],Ti[1][3],Ti[2][3],206,239),eomg=0.0001,ev=0.0001) animarTrajetoria(Ti,Tf,10) for i in range(6): if Ik[0][i] > 0: while Ik[0][i] > np.pi*2: Ik[0][i] = Ik[0][i] - np.pi*2 if Ik[0][i] < 0: while Ik[0][i] < -np.pi*2: Ik[0][i] = Ik[0][i] + np.pi*2 for i in range(6): Ik[0][i] = round(np.rad2deg(Ik[0][i]),0) Ik[0][i] = float(Ik[0][i])
def main(): clientID = startSimulation() # Handles joint_handles = [-1, -1, -1, -1, -1, -1] for i in range(0, 6): joint_handles[i] = sim.simxGetObjectHandle(clientID, 'UR3_joint' + str(i + 1), sim.simx_opmode_blocking)[1] base_handle = sim.simxGetObjectHandle(clientID, "UR3", sim.simx_opmode_blocking)[1] end_effector_handle = sim.simxGetObjectHandle(clientID, "UR3_link7_visible", sim.simx_opmode_blocking)[1] end_effector_wrt_base = sim.simxGetObjectPosition( clientID, end_effector_handle, base_handle, sim.simx_opmode_streaming)[1] end_effector_quat_wrt_base = sim.simxGetObjectQuaternion( clientID, end_effector_handle, base_handle, sim.simx_opmode_streaming)[1] time.sleep(0.5) # Get end effector position and rotation wrt base end_effector_wrt_base = sim.simxGetObjectPosition( clientID, end_effector_handle, base_handle, sim.simx_opmode_oneshot_wait)[1] end_effector_quat_wrt_base = sim.simxGetObjectQuaternion( clientID, end_effector_handle, base_handle, sim.simx_opmode_oneshot_wait)[1] R_end_effector_wrt_base = Rotation.from_quat( end_effector_quat_wrt_base).as_matrix() P_end_effector_wrt_base = end_effector_wrt_base M = mr.RpToTrans(R_end_effector_wrt_base, P_end_effector_wrt_base) print(M) # Forward Kinematics Slist = getSlist(clientID, joint_handles, base_handle) theta = [ degToRad(180), degToRad(0), degToRad(0), degToRad(0), degToRad(0), degToRad(0) ] T_endeff_in_base = mr.FKinSpace(M, Slist.T, theta) print(T_endeff_in_base) success = False while (not success): theta_new, success = mr.IKinSpace(Slist.T, M, T_endeff_in_base, [ random.random(), random.random(), random.random(), random.random(), random.random(), random.random() ], 0.01, 0.01) time.sleep(1) move.setTargetPosition(clientID, joint_handles, theta_new) errorCode = 1 while (errorCode): errorCode, end_effector_wrt_base = sim.simxGetObjectPosition( clientID, end_effector_handle, base_handle, sim.simx_opmode_oneshot_wait) print(end_effector_wrt_base) time.sleep(1) move.setTargetPosition(clientID, joint_handles, [0, 0, 0, 0, 0, 0]) endSimulation(clientID) return 1
def inverseKin(final_endeffector, thetas): #thetas = thetas*np.pi/180 #print(thetas) tolerances = 0.0001 M =np.array([[1,0,0,-0.2700],\ [0,1,0,0.01],\ [0,0,1,0.652],\ [0,0,0,1]]) W = np.array([[0, -1, -1, -1, 0, -1],\ [ 0, 0, 0, 0, 0, 0],\ [ 1, 0, 0, 0, 1, 0]]) #using lab measurements Q =np.array([[0.0,-0.120,-0.120,-0.027,-0.110,-0.110],\ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],\ [0.0, 0.152, 0.396, 0.609, 0.609, 0.692]]) V = [[0,0,0,0,0,0],\ [0,0,0,0,0,0],\ [0,0,0,0,0,0]] #v=cross(-w,q) for idx in range(0, 6): V[0][idx] = -W[1][idx] * Q[2][idx] + W[2][idx] * Q[1][idx] V[1][idx] = -W[2][idx] * Q[0][idx] + W[0][idx] * Q[2][idx] V[2][idx] = -W[0][idx] * Q[1][idx] + W[1][idx] * Q[0][idx] S =np.array([[W[0][0],W[0][1],W[0][2],W[0][3],W[0][4],W[0][5]],\ [W[1][0],W[1][1],W[1][2],W[1][3],W[1][4],W[1][5]],\ [W[2][0],W[2][1],W[2][2],W[2][3],W[2][4],W[2][5]],\ [V[0][0],V[0][1],V[0][2],V[0][3],V[0][4],V[0][5]],\ [V[1][0],V[1][1],V[1][2],V[1][3],V[1][4],V[1][5]],\ [V[2][0],V[2][1],V[2][2],V[2][3],V[2][4],V[2][5]]]) #print('S=',S,'\n','M=',M,'\n','final_endeffector=',final_endeffector,'\n','tol=',tolerances,'\n',) theta, success = mr.IKinSpace(S, M, final_endeffector, thetas, tolerances, tolerances) if (success == False): print( 'S=', S, '\n', 'M=', M, '\n', 'final_endeffector=', final_endeffector, '\n', 'tol=', tolerances, '\n', ) print('Warning not in tolerances \n ') res = input('Continue? y/n:') if (res == 'n'): print('press ctrl + c') while (1): a = 1 theta_return = theta #np.degrees(theta) #print(theta_return) return (theta_return)