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)
