in robots/LoCoBot/locobot_control/scripts/locobot_control/analytic_ik.py [0:0]
def ik(self, position, alpha=0.0, cur_arm_config=4 * [0]):
"""
# x,y,z are end effector pose with respect to the base link
:param position: cartesian param in m [x,y,z]
:param alpha: float (in radians)alpha is the last joint angle wrt ground that you want.. upward +ve
:param cur_arm_config: list config of arm, this will be helpful for selecting one out of multiple solutions
:return: list if found the solution otherwise None
"""
trans = copy.deepcopy(position)
assert len(trans) == 3
for i in range(3):
trans[i] = trans[i] - self.base_offset[i]
(x, y, z) = tuple(trans)
# for holding final solution
theta = [0.0, 0.0, 0.0, 0.0]
# 1) Convert it into cylindrical system and find base angle
theta[0] = math.atan2(y, x)
if not self.joint_limits[0][0] < theta[0] < self.joint_limits[0][1]:
return None
# 2) As X,Y,Z & alpha for end-effector is given, therefore J4 location is fixed. now we neet to find out J3 & J2
# location and use the locations to get the joint angles using circle circle intersection
X = math.sqrt(x ** 2 + y ** 2)
Y = z
p3 = (X - self.l4 * math.cos(alpha), Y - self.l4 * math.sin(alpha))
circle1 = (0, self.l1, self.l2)
circle2 = (p3[0], p3[1], self.l3)
inter = self.circle_intersection(circle1, circle2)
# if found the solution
if inter:
inter1 = inter[0]
inter2 = inter[1]
else:
return None
# if there is intersection then 2 possible solution will be possible
angle1 = (
math.atan2(inter1[0], inter1[1] - self.l1) - self.angle2_bias,
math.atan2(inter2[0], inter2[1] - self.l1) - self.angle2_bias,
)
angle2 = (
-math.atan2(p3[1] - inter1[1], p3[0] - inter1[0]) - angle1[0],
-math.atan2(p3[1] - inter2[1], p3[0] - inter2[0]) - angle1[1],
)
angle3 = (-alpha - angle1[0] - angle2[0], -alpha - angle1[1] - angle2[1])
# filter out one angle
# 1) if both found solutions are in range select the one which is closest to current configuration of robot
if (
all(self.joint_limits[1][0] < i < self.joint_limits[1][1] for i in angle1)
and all(
self.joint_limits[2][0] < i < self.joint_limits[2][1] for i in angle2
)
and all(
self.joint_limits[3][0] < i < self.joint_limits[3][1] for i in angle3
)
):
# take the one which is closer to the current arm configuration
diff_0 = (
abs(angle1[0] - cur_arm_config[1])
+ abs(angle2[0] - cur_arm_config[2])
+ abs(angle3[0] - cur_arm_config[3])
)
diff_1 = (
abs(angle1[1] - cur_arm_config[1])
+ abs(angle2[1] - cur_arm_config[2])
+ abs(angle3[1] - cur_arm_config[3])
)
if diff_0 < diff_1:
theta[1] = angle1[0]
theta[2] = angle2[0]
theta[3] = angle3[0]
return theta
else:
theta[1] = angle1[1]
theta[2] = angle2[1]
theta[3] = angle3[1]
return theta
# 2) which ever satisfy constraints
elif (
self.joint_limits[1][0] < angle1[0] < self.joint_limits[1][1]
and self.joint_limits[2][0] < angle2[0] < self.joint_limits[2][1]
and self.joint_limits[3][0] < angle3[0] < self.joint_limits[3][1]
):
theta[1] = angle1[0]
theta[2] = angle2[0]
theta[3] = angle3[0]
return theta
elif (
self.joint_limits[1][0] < angle1[1] < self.joint_limits[1][1]
and self.joint_limits[2][0] < angle2[1] < self.joint_limits[2][1]
and self.joint_limits[3][0] < angle3[1] < self.joint_limits[3][1]
):
theta[1] = angle1[1]
theta[2] = angle2[1]
theta[3] = angle3[1]
return theta
else:
return None