class KSOLVER: def __init__(self, *args, **kwargs): self.chain = Chain( name='left_arm', links=[ OriginLink(), URDFLink( name="shoulder_left", translation_vector=[0.13182, -2.77556e-17, 0.303536], orientation=[-0.752186, -0.309384, -0.752186], rotation=[0.707107, 0, 0.707107], ), URDFLink( name="proximal_left", translation_vector=[-0.0141421, 0, 0.0424264], orientation=[-7.77156e-16, 7.77156e-16, 5.55112e-17], rotation=[-0.707107, 0, 0.707107], ), URDFLink( name="distal_left", translation_vector=[0.193394, -0.0097, 0.166524], orientation=[1.66533e-16, -7.21645e-16, 3.88578e-16], rotation=[0, -1, 0], ) ]) def solve(self, wrist_postion): """Performs inverse kinematics and returns joint angles in rads For now uses one target vector at the wrist position for inverse kinematics. For the future, should use additional target vectors and match the elbow """ # TODO: Fix coordinate system transform for extra degree of freedom rotation # Setup target vector with target position at wrist target_vector = np.array( [wrist_postion[0], wrist_postion[1], wrist_postion[2]]) target_frame = np.eye(4) target_frame[:3, 3] = target_vector joint_angles = self.chain.inverse_kinematics(target_frame)[1:3] print('Joint angles:', joint_angles) print('Computed position vector:', self.chain.forward_kinematics(self.chain.inverse_kinematics(target_frame))[:3, 3], \ 'Original position vector:', target_frame[:3, 3]) joints = self.chain.inverse_kinematics(target_frame)[1:3] return np.array([0, joints[1]])
def solve_shoulder_angles(v_o, v_new, a1, a2, b1, b2): chain = Chain( name='arm', links=[ #OriginLink(), URDFLink( name="shoulder1", translation_vector=[0, 0, 0], orientation=[0, 0, 0], bounds=b1, rotation=a1, ), URDFLink( name="shoulder2", translation_vector=[0, 0, 0], orientation=[0, 0, 0], bounds=b2, rotation=a2, ), URDFLink( name="elbow", translation_vector=v_o, orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) target_frame = np.eye(4) target_frame[:3, 3] = v_new angles = chain.inverse_kinematics(target_frame)[:2] return np.rad2deg(angles)
class ArmIK: def __init__(self): self.chain = Chain( name="pupper_ergo", links=[ OriginLink(), URDFLink( name="twistybase", translation_vector=[0, 0, 0.03], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-90, 90), ), URDFLink( name="shoulder", translation_vector=[0, 0, 0.0285], orientation=[0, 0, 0], rotation=[1, 0, 0], bounds=(-90, 90), ), URDFLink( name="elbow", translation_vector=[0, 0, 0.085], orientation=[0, 0, 0], rotation=[1, 0, 0], bounds=(-90, 90), ), URDFLink( name="tip", translation_vector=[0, 0.155, 0], orientation=[1.57, 0, 0], rotation=[1, 0, 0], bounds=(-90, 90), ), ], ) @staticmethod def _ik2pupperarm(joints): return np.rad2deg([joints[1], -joints[2], -joints[3], 0.0]) @staticmethod def _fk2pupperarm(joints): return np.deg2rad([joints[1], -joints[2], -joints[3], 0.0, 0.0]) @staticmethod def _makeMoveCmd(degs): print(f"move([{','.join([str(x) for x in np.around(degs, 2)])}])") def pos2joints(self, target): joints = self.chain.inverse_kinematics(target) return self._ik2pupperarm(joints) def joints2pos(self, joints): joints = self._fk2pupperarm(joints) return self.chain.forward_kinematics(joints)[:3, 3]
class Arm: def __init__(self, direction): self.state = [0, 0, 0, 0, 0] if direction == "left": sign = -1 else: sign = 1 self.chain = Chain( name=direction + "_arm", links=[ OriginLink(), URDFLink( name="shoulder", translation_vector=[8 * sign, 0, 0], orientation=[0, 0, math.radians(30 * sign)], rotation=[0, 0, 1], ), URDFLink(name="backarm", translation_vector=[16 * sign, 0, 0], orientation=[0, 0, math.radians(90 * sign)], rotation=[0, 0, 1]), URDFLink(name="forearm", translation_vector=[16 * sign, 0, 0], orientation=[0, 0, math.radians(30 * sign)], rotation=[0, 0, 1]), URDFLink(name="hand", translation_vector=[9 * sign, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1]), ]) def get_positions(self): rv = [] positions = self.chain.forward_kinematics(self.state, full_kinematics=True) for position in positions[1:]: xyEtc = position[:, 3] x = xyEtc[0] y = xyEtc[1] rv.append((x, y)) return rv def go_directly_to_position(self, x, y, book): rad = math.radians(book.openness / 2) if (x < book.center_x_cm): rad *= -1 joint_positions = self.chain.inverse_kinematics( target_position=[x, y, 0], target_orientation=[0, 0, 0.2]) self.state = joint_positions
def solve_hip_angles(v_o, v_new, a1, a2, a3, b1, b2, b3): chain = Chain( name='arm', links=[ #OriginLink(), URDFLink( name="hip1", translation_vector=[0, 0, 0], orientation=[0, 0, 0], bounds=b1, rotation=a1, ), URDFLink( name="hip2", translation_vector=[0, 0, 0], orientation=[0, 0, 0], bounds=b2, rotation=a2, ), URDFLink( name="hip3", translation_vector=[0, 0, 0], orientation=[0, 0, 0], bounds=b3, rotation=a3, ), URDFLink( name="knee", translation_vector=v_o, orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) target_frame = np.eye(4) target_frame[:3, 3] = v_new angles = chain.inverse_kinematics(target_frame)[:3] return np.rad2deg(angles)
] robo_chain = Chain(name="robo_chain", links=ulinks) robo_chain.forward_kinematics([0] * len(ulinks)) # test matrix, to check if defaults are as expected: look_straight = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]] # translate in all directions tf1 = [[1, 0, 0, 2], [0, 1, 0, 2], [0, 0, 1, 2], [0, 0, 0, 1]] # rotate along all axes tf2 = [[0.7, 0, 0, 2], [0, 0.7, 0, 2], [0, 0, 0.7, 2], [0, 0, 0, 1]] #robo__chain.inverse_kinematics(look_straight) import time #time.sleep(1000) if __name__ == '__main__': # show inverse kinematic import matplotlib.pyplot from mpl_toolkits.mplot3d import Axes3D for tf in [look_straight, tf1, tf2]: ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') robo_chain.plot(robo_chain.inverse_kinematics(tf), ax) matplotlib.pyplot.show() # close current plot, to show the next!
def ik(self, target_position=None, target_orientation=None, orientation_mode=None, **kwargs): full = Chain.inverse_kinematics(self, target_position, target_orientation, orientation_mode, **kwargs) active = self.joints_from_links(full) return active
URDFLink( name="EOF", translation_vector=[0.5, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], # bounds = (0, 0) ) ]) # blue # target_vector = [0.4, 0.8, .2] target_frame = np.eye(4) target_frame[:3, 3] = target_vector angles = my_chain.inverse_kinematics(target_frame) for (i, angle) in enumerate(angles): angles[i] = math.degrees(angle) print("angle({}) = {} deg, {} pos".format(i, angles[i], int(angles[i] / 0.24))) real_frame = my_chain.forward_kinematics( my_chain.inverse_kinematics(target_frame)) print("Computed position vector : %s, original position vector : %s" % (real_frame[:3, 3], target_frame[:3, 3])) import matplotlib.pyplot as plt ax = plot_utils.init_3d_figure() my_chain.plot(my_chain.inverse_kinematics(target_frame),
URDFLink( name="joint3", translation_vector=[0.045, 0, 0.802], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="joint4", translation_vector=[-0.045, 0, 0.746], orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) print("start") angle = my_chain.inverse_kinematics([ [1, 0, 0, 0], [0, 1, 0, 0.3], [0, 0, 1, 1], [0, 0, 0, 1] ]) print(angle) angle = my_chain.inverse_kinematics([ [1, 0, 0, 0], [0, 1, 0, 0.5], [0, 0, 1, 1], [0, 0, 0, 1] ]) print(angle)
# [0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 1,1,1,1,1], # LL # [0,0,0,0,0, 1,1,1,1,1, 0,0,0,0,0, 0,0,0,0,0], # RL # [0,0,0,0,0, 0,0,0,0,0, 1,1,1,1,1, 0,0,0,0,0], # LA # [1,1,1,1,1, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0], # RA # Phase shift between arms/legs, [RA, RL, LA, LL] shiftFrame = np.round(np.array([0, pi / 2, pi, 3 * pi / 2]) / 2 / pi * frames) #shiftFrame = np.round(np.array([0, 0, pi, pi])/2/pi*frames) #shiftFrame = np.round(np.array([0, pi, pi, 0])/2/pi*frames) shiftFrame = shiftFrame.astype(int) for frame in range(0, frames): right_arm_angles_raw = right_arm.inverse_kinematics( target_position=np.array([ stepLength * longitudinalMovement[frame], -bodyWidth / 2, swingHeight * verticalMovement[frame] ])) right_arm_correction = np.array([0, -pi / 2, pi / 2, 0]) right_arm_angles = np.round(rad2deg * (right_arm_angles_raw + right_arm_correction)) right_arm_angles = np.delete(right_arm_angles, np.s_[0, 3], axis=0) right_arm_angles = right_arm_angles.astype(int) right_leg_angles_raw = right_leg.inverse_kinematics( target_position=np.array([ -bodyLength + stepLength * longitudinalMovement[frame + shiftFrame[1]], -bodyWidth / 2, swingHeight * verticalMovement[frame + shiftFrame[1]] ])) right_leg_correction = np.array([0, 0, -pi / 2, -pi / 2, 0])
# take the coordinate of the targer # x_input = float(input("X: ")) y_input = float(input("Y: ")) z_input = float(input("Z: ")) # initiate the target vector and the target frame # x = x_input #* SCALER y = y_input #* SCALER z = z_input #* SCALER target_vector = [x, y, z] target_frame = np.eye(4) target_frame[:3, 3] = target_vector angles = arm.inverse_kinematics(target_frame) for (i, angle) in enumerate(angles): angles[i] = math.degrees(angle) if (i == 0): angles[i] = angles[i] + 30 elif (i == 3 and angles[i] == 0): # FIXME: try and fix this angles[i] = angles[i] + 180 # if (angles[i] > 360 ): # angles[i] = angles[i] % 360 print("angle({}) = {} deg, {} pos".format(i, angles[i], int(angles[i] / 0.24)))
name="joint1", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="joint2", translation_vector=[0, 0, 0.075], orientation=[0, 0, 0], rotation=[1, 0, 0], ), URDFLink( name="joint3", translation_vector=[0.045, 0, 0.802], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="joint4", translation_vector=[-0.045, 0, 0.746], orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) my_chain.plot( my_chain.inverse_kinematics([[1, 0, 0, -0.3], [0, 1, 0, 0.3], [0, 0, 1, 1], [0, 0, 0, 1]]), ax) #my_chain.plot([0,0,0],ax) matplotlib.pyplot.show()
orientation=[0, 0, 0], rotation=[0, 0, 1], ) ]) # positions = robotic_arm_chain.forward_kinematics([0] * 4) # print("Positions: \n{}".format(positions)) # # # target_vector = [0.1, -0.2, 0.1] # target_vector = [0.5, 0.5, 0.0] # target_frame = geometry_utils.to_transformation_matrix( # target_vector, # np.eye(3)) # print("The angles of each joints are : ", robotic_arm_chain.inverse_kinematics(target_frame)) # real_frame = robotic_arm_chain.forward_kinematics(robotic_arm_chain.inverse_kinematics(target_frame)) # print("Computed position vector : %s, original position vector : %s" % (real_frame[:3, 3], target_frame[:3, 3])) # # # ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') # end = robotic_arm_chain.inverse_kinematics(target_frame) # robotic_arm_chain.plot(end, ax, target=target_vector) # # # matplotlib.pyplot.xlim(-0.2, 0.2) # # matplotlib.pyplot.ylim(-0.2, 0.2) # matplotlib.pyplot.title("Target: {}, end: {}".format(target_vector, end)) # matplotlib.pyplot.show() ik = robotic_arm_chain.inverse_kinematics([[1, 0, 0, 2], [0, 1, 0, 2], [0, 0, 1, 2], [0, 0, 0, 1]]) print("ik: {}".format(ik))
name="shoulder", translation_vector=[-10, 0, 5], orientation=[0, 1.57, 0], rotation=[0, 1, 0], ), URDFLink( name="elbow", translation_vector=[25, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="wrist", translation_vector=[22, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ) ]) target_position = [2, 2, 2] print( left_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix(target_position, np.eye(3)))) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') left_arm_chain.plot(left_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix( [2, 2, 2], [[1, 0, 0], [0, 1, 0], [0, 0, 1]])), ax, target=target_position) matplotlib.pyplot.show()
class InverseKinematics(): def __init__(self): # Second robo, with dynamixel self.armLength = 4 self.smallArmLength = 4 lim = math.pi / 4 # lim = 7*math.pi/9 self.chain = Chain( name='arm', links=[ OriginLink(), URDFLink(name="base", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-math.pi, math.pi)), URDFLink(name="first", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], bounds=(-lim, lim)), URDFLink(name="second", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, -1, 0], bounds=(-lim, lim)), URDFLink(name="third", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, 1, 0], bounds=(-lim, lim)), URDFLink(name="fourth", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, -1, 0], bounds=(-lim, lim)), URDFLink( name="tip", translation_vector=[0, 0, self.smallArmLength], orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) self.xMin = -3 * self.armLength - self.smallArmLength self.xMax = 3 * self.armLength + self.smallArmLength self.yMin = -3 * self.armLength - self.smallArmLength self.yMax = 3 * self.armLength + self.smallArmLength self.zMin = 0 self.zMax = 3 * self.armLength + self.smallArmLength self.radToDegreeFactor = 180 / math.pi self.degreeToRadFactor = math.pi / 180 self.numberOfLinks = len(self.chain.links) # def __init__(self): # # First robo, with MG995 Servos # self.armLength = 10 # self.smallArmLength = 5 # self.chain = Chain(name='arm', links=[ # OriginLink( # ), # URDFLink( # name="base", # translation_vector=[0, 0, 0], # orientation=[0, 0, 0], # rotation=[0, 0, 1], # bounds=(-math.pi,math.pi) # ), # URDFLink( # name="first", # translation_vector=[0, 0, 0], # orientation=[0, 0, 0], # rotation=[0, 1, 0], # bounds=(-4*math.pi/9,4*math.pi/9) # ), # URDFLink( # name="second", # translation_vector=[0, 0, self.armLength], # orientation=[0, 0, 0], # rotation=[0, -1, 0], # bounds=(-4*math.pi/9,4*math.pi/9) # ), # URDFLink( # name="third", # translation_vector=[0, 0, self.armLength], # orientation=[0, 0, 0], # rotation=[0, 1, 0], # bounds=(-4*math.pi/9,4*math.pi/9) # ), # URDFLink( # name="fourth", # translation_vector=[0, 0, self.armLength], # orientation=[0, 0, 0], # rotation=[0, -1, 0], # bounds=(-4*math.pi/9,4*math.pi/9) # ), # URDFLink( # name="tip", # translation_vector=[0, 0, self.smallArmLength], # orientation=[0, 0, 0], # rotation=[0, 0, 0], # ) # ]) # self.xMin = -3*self.armLength - self.smallArmLength # self.xMax = 3*self.armLength + self.smallArmLength # self.yMin = -3*self.armLength - self.smallArmLength # self.yMax = 3*self.armLength + self.smallArmLength # self.zMin = 0 # self.zMax = 3*self.armLength + self.smallArmLength # self.radToDegreeFactor = 180 / math.pi # self.degreeToRadFactor = math.pi / 180 # self.numberOfLinks = len(self.chain.links) def __del__(self): pass def getPosition(self, angles, allLinks=False): anglesRad = [] if not allLinks: anglesRad.append(0) for angle in angles: anglesRad.append(angle * self.degreeToRadFactor) if not allLinks: anglesRad.append(0) # print("anglesRad: {}".format(anglesRad)) calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3] calcPosPerc = [] calcPosPerc.append( myutils.map(calcPos[0], self.xMin, self.xMax, -100, 100)) calcPosPerc.append( myutils.map(calcPos[1], self.yMin, self.yMax, -100, 100)) calcPosPerc.append( myutils.map(calcPos[2], self.zMin, self.zMax, 0, 100)) return calcPosPerc def getAngles(self, pos): if pos[0] == 0: pos[0] = 0.01 if pos[1] == 0: pos[1] = 0.01 if pos[2] == 0: pos[2] = 0.01 pos[0] = myutils.map(pos[0], 0, 100, self.xMin, self.xMax) pos[1] = myutils.map(pos[1], 0, 100, self.yMin, self.yMax) pos[2] = myutils.map(pos[2], 0, 100, self.zMin, self.zMax) anglesRad = self.chain.inverse_kinematics(pos) angles = anglesRad * self.radToDegreeFactor calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3] calcPosPerc = self.getPosition(angles, True) newAngles = [] for i in range(1, self.numberOfLinks - 1): # newAngles.append(math.floor(angles[i])) newAngles.append(angles[i]) outOfRange = False for i in range(len(calcPosPerc)): if abs(calcPosPerc[i] - pos[i]) > 2: outOfRange = True break log("{} calcPosPerc {}".format(outOfRange, calcPosPerc), "OK") print("Angles are: {}".format(newAngles)) return newAngles, outOfRange def getAnglesRaw(self, pos): if pos[0] == 0: pos[0] = 0.01 if pos[1] == 0: pos[1] = 0.01 if pos[2] == 0: pos[2] = 0.01 corrPos = [] corrPos.append(pos[0] * self.xMax / 100) corrPos.append(pos[1] * self.yMax / 100) corrPos.append(pos[2] * self.zMax / 100) anglesRad = self.chain.inverse_kinematics(corrPos) angles = anglesRad * self.radToDegreeFactor startTime = time.time() calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3] log("elapsed timeO: {}".format(time.time() - startTime)) calcPosPerc = self.getPosition(angles, True) newAngles = [] for i in range(1, self.numberOfLinks - 1): newAngles.append(angles[i]) outOfRange = False for i in range(len(calcPosPerc)): if abs(calcPosPerc[i] - pos[i]) > 2: outOfRange = True break log("all Angles: {}".format(angles)) log("Out of range: {}, calcPosPerc {}".format(outOfRange, calcPosPerc), "OK") log("set pos: {}".format(pos), "OK") log("Angles are: {}".format(newAngles)) return newAngles, outOfRange
rotation=[0, 0, 0], ) ], active_links_mask=[False, True, True, True]) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') target_vector = [0.1, -0.2, 0.1] l_wrist_matrix = geometry_utils.to_transformation_matrix(target_vector) target_frame = np.eye(4) target_frame[:3, 3] = target_vector left_arm_start_position = left_arm_chain.forward_kinematics([0] * 4) right_arm_start_position = right_arm_chain.forward_kinematics([0] * 4) head_start_position = head_chain.forward_kinematics([0] * 3) left_leg_start_position = left_leg_chain.forward_kinematics([0] * 4) right_leg_start_position = right_leg_chain.forward_kinematics([0] * 4) print(right_arm_start_position) left_arm_chain.plot(left_arm_chain.inverse_kinematics(l_wrist_matrix), ax, target=target_vector) right_arm_chain.plot( right_arm_chain.inverse_kinematics(right_arm_start_position), ax) head_chain.plot(head_chain.inverse_kinematics(head_start_position), ax) left_leg_chain.plot(left_leg_chain.inverse_kinematics(left_leg_start_position), ax) right_leg_chain.plot( right_leg_chain.inverse_kinematics(right_leg_start_position), ax) matplotlib.pyplot.show()
class Control: """ Realization of continuous actions, from world model to desired world. """ def __init__(self, init_world_model): self.control_world_model = init_world_model.current_world_model.control self.arm_url = init_world_model.current_world_model.url["arm"] self.cm_to_servo_polynomial_fitter = \ joblib.load(init_world_model.current_world_model.control["cm_to_servo_polynomial_fitter"]["file_path"]) self.init_position = np.array(self.control_world_model["init_position"] ) * self.control_world_model["scale"] self.link_bounds = tuple( np.radians( np.array([ -self.control_world_model["angle_degree_limit"], self.control_world_model["angle_degree_limit"] ]))) # In degrees self.le_arm_chain = Chain( name=self.control_world_model["chain_name"], active_links_mask=self.control_world_model["active_links_mask"], links=[ URDFLink( name="link6", translation_vector=np.array( self.control_world_model["link_lengths"]["link6"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link6"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link6"]), bounds=self.link_bounds), URDFLink( name="link5", translation_vector=np.array( self.control_world_model["link_lengths"]["link5"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link5"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link5"]), bounds=self.link_bounds), URDFLink( name="link4", translation_vector=np.array( self.control_world_model["link_lengths"]["link4"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link4"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link4"]), bounds=self.link_bounds), URDFLink( name="link3", translation_vector=np.array( self.control_world_model["link_lengths"]["link3"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link3"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link3"]), bounds=self.link_bounds), URDFLink( name="link2", translation_vector=np.array( self.control_world_model["link_lengths"]["link2"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link2"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link2"]), bounds=self.link_bounds), URDFLink( name="link1", translation_vector=np.array( self.control_world_model["link_lengths"]["link1"]) * self.control_world_model["scale"], orientation=self.control_world_model["link_orientations"] ["link1"], rotation=np.array( self.control_world_model["joint_rotation_axis"] ["link1"]), bounds=self.link_bounds) ]) def xyz_to_servo_range(self, xyz, current_servo_monotony): """ Converts 3D cartesian centimeter coordinates to servo values in [500, 2500]. :param xyz: Array of 3 elements of a 3D cartesian systems of centimeters. :param current_servo_monotony: List of 6 positive or negative servo rotation directions. :return: List of 6 servo values in [500, 2500]. """ k = self.le_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix(xyz, np.eye(3))) k = np.multiply(k, np.negative(current_servo_monotony)) return self.radians_to_servo_range(k) def servo_range_to_xyz(self, servo_range, current_servo_monotony): """ Converts servo values in [500, 2500] to 3D cartesian centimeter coordinates. :param servo_range: List of 6 servo values in [500, 2500]. :param current_servo_monotony: List of 6 positive or negative servo rotation directions. :return: Array of 3 elements of a 3D cartesian systems of centimeters. """ return geometry_utils.from_transformation_matrix( self.le_arm_chain.forward_kinematics( np.multiply(self.servo_range_to_radians(servo_range), np.negative(current_servo_monotony)), ))[0][:3] @staticmethod def servo_range_to_radians(x, x_min=500.0, x_max=2500.0, scaled_min=(-np.pi / 2.0), scaled_max=(np.pi / 2.0)): """ Converts servo values in [500, 2500] to angle radians. :param x: List of 6 servo values. :param x_min: Scalar float, minimum servo value of 90 degrees angle (default = 500). :param x_max: Scalar float, maximum servo value of 90 degrees angle(default = 2500). :param scaled_min: Scalar float, minimum radians value of +90 degrees angle(default = -π/2). :param scaled_max: Scalar float, maximum radians value of +90 degrees angle(default = π/2). :return: List of 6 angles in radians. """ x_std = (np.array(x) - x_min) / (x_max - x_min) return x_std * (scaled_max - scaled_min) + scaled_min @staticmethod def radians_to_servo_range(x, x_min=(-np.pi / 2.0), x_max=(np.pi / 2.0), scaled_min=500.0, scaled_max=2500.0): """ Converts angle radians to servo values in [500, 2500]. :param x: List of 6 angles in radians. :param x_min: Scalar float, minimum radians value of +90 degrees angle(default = -π/2). :param x_max: Scalar float, maximum radians value of +90 degrees angle(default = π/2). :param scaled_min: Scalar float, minimum servo value of 90 degrees angle (default = 500). :param scaled_max: Scalar float, maximum servo value of 90 degrees angle(default = 2500). :return: List of 6 servo values. """ x_std = (np.array(x) - x_min) / (x_max - x_min) return (np.round(x_std * (scaled_max - scaled_min) + scaled_min, 0)).astype(int) def get_kinematic_angle_trajectory(self, from_angle_radians_in, to_angle_radians_in, servo_monotony, steps=10): """ Creates a discrete end-effector trajectory, using radians. :param from_angle_radians_in: Current servo angles, list of 6 angles in radians. :param to_angle_radians_in: Desired servo angles, list of 6 angles in radians. :param servo_monotony: List of 6 positive or negative servo rotation directions. :param steps: Scalar integer, the total steps for the end effector trajectory. :return: List of end-effector radian trajectory steps. """ assert self.control_world_model[ "min_steps"] < steps < self.control_world_model["max_steps"] from_angle_radians = np.multiply(from_angle_radians_in, servo_monotony) to_angle_radians = np.multiply(to_angle_radians_in, servo_monotony) step_angle_radians = [] for index in range(len(to_angle_radians_in)): step_angle_radians.append( (from_angle_radians[index] - to_angle_radians[index]) / float(steps)) angle_trajectory = [] step_angle_radians = np.array(step_angle_radians) current_angles = np.array(from_angle_radians) # angle_trajectory.append(current_angles) for _ in range(steps): current_angles = np.add(current_angles, step_angle_radians) angle_trajectory.append(current_angles) return angle_trajectory def get_servo_range_trajectory(self, from_servo_range_in, to_servo_range_in, steps=10): """ Creates a discrete end-effector trajectory, using servo values. :param from_servo_range_in: Current servo values, list of 6 values in [500, 2500]. :param to_servo_range_in: Desired servo values, list of 6 values in [500, 2500]. :param steps: Scalar integer, the total steps for the end effector trajectory. :return: List of end-effector servo value trajectory steps. """ assert self.control_world_model[ "min_steps"] < steps < self.control_world_model["max_steps"] from_servo_range = np.array(from_servo_range_in) to_servo_range = np.array(to_servo_range_in) if self.control_world_model["verbose"]: print("from_servo_range: ", from_servo_range) print("to_servo_range: ", to_servo_range) step_servo_range = [] for index in range(len(to_servo_range)): step_servo_range.append( (to_servo_range[index] - from_servo_range[index]) / float(steps)) if self.control_world_model["verbose"]: print("step_servo_range: ", step_servo_range) servo_range_trajectory = [] step_servo_range = np.array(step_servo_range) current_servo_range = np.array(from_servo_range) for _ in range(steps): current_servo_range = np.add(current_servo_range, step_servo_range) servo_range_trajectory.append(current_servo_range) return np.array(np.round(servo_range_trajectory, 0)).astype(int) def initialize_arm(self, last_servo_values): """ Moves the end-effector to the (0, 0, 0) position of the 3d cartesian. :param last_servo_values: List of the current arm servo positions. :return: True if succeeded. """ action_successful = False target_position = np.array( self.init_position) * self.control_world_model["scale"] action_successful = self.move_arm(target_position, last_servo_values) print("=== Arm initialized") return action_successful def move_arm_above_xyz(self, xyz, last_servo_values, height): """ Moves the end-effector at a specific 3D cartesian centimeter position, plus extra centimeters high. :param xyz: Array of 3 elements of a 3D cartesian systems of centimeters. :param last_servo_values: List of the current arm servo positions. :param height: Scalar positive float. Desired centimeters above xyz, on the z axis. :return: True if succeeded. """ action_successful = False xyz[2] = height target_position = np.array(xyz) * self.control_world_model["scale"] if self.control_world_model["send_requests"]: action_successful = self.move_arm(target_position, last_servo_values) print("=== Arm above object") return action_successful def move_arm_up(self, last_servo_values, height): """ Moves the end-effector at a specific 3D cartesian centimeter position, plus extra centimeters high. :param last_servo_values: List of the current arm servo positions. :param height: Scalar positive float. Desired centimeters above xyz, on the z axis. :return: True if succeeded. """ action_successful = False xyz = np.round( self.servo_range_to_xyz( last_servo_values, self.control_world_model["current_servo_monotony"]), 2) print("last_servo_xyz", xyz) xyz[2] = height target_position = np.array(xyz) * self.control_world_model["scale"] if self.control_world_model["send_requests"]: action_successful = self.move_arm(target_position, last_servo_values) print("=== Arm up") return action_successful def move_arm_to_object(self, xyz, last_servo_values): """ Moves the end-effector to the object's position of the 3d cartesian. :param xyz: Array of 3 elements of a 3D cartesian systems of centimeters. :param last_servo_values: List of the current arm servo positions. :return: True if succeeded. """ action_successful = False target_position = np.array(xyz) * self.control_world_model["scale"] if self.control_world_model["send_requests"]: action_successful = self.move_arm(target_position, last_servo_values) print("=== Arm to object") return action_successful def close_hand(self, object_side_length): """ Closes the gripper enough, to grip an object of a specific length in cm. :param object_side_length: Scalar float, object width in centimeters. :return: True if succeeded. """ action_successful = False closed_length = object_side_length * self.control_world_model[ "closed_hand_distance_ratio"] servo_range = int(self.cm_to_servo_polynomial_fitter(closed_length)) if self.control_world_model["verbose"]: print("cm: {}, predicted servo value: {}".format( closed_length, servo_range)) if self.control_world_model["send_requests"]: action_successful = self.send_restful_servo_range( self.control_world_model["gripping_gripper_servo"], servo_range) print("=== Gripper closed") return action_successful def open_hand(self, object_side_length): """ Opens the gripper enough, to fit an object of a specific length in cm. :param object_side_length: Scalar float, object width in centimeters. :return: True if succeeded. """ action_successful = False opened_length = object_side_length * self.control_world_model[ "opened_hand_distance_ratio"] servo_range = int(self.cm_to_servo_polynomial_fitter(opened_length)) if self.control_world_model["verbose"]: print("cm: {}, predicted servo value: {}".format( opened_length, servo_range)) if self.control_world_model["send_requests"]: action_successful = self.send_restful_servo_range( self.control_world_model["gripping_gripper_servo"], servo_range) print("=== Gripper opened") return action_successful def send_restful_servo_range(self, servo, in_range): """ Sends a direct servo value in [500, 2500], to a specific servo in [1, 6]. :param servo: Scalar integer, the servo id in [1, 6]. :param in_range: Scalar integer, servo value in [500, 2500]. :return: True if succeeded. """ action_successful = False url = self.control_world_model["base_put_url"].format( self.arm_url, servo, in_range) requests.put(url, data="") time.sleep(self.control_world_model["command_delay"]) action_successful = True return action_successful def send_restful_trajectory_requests(self, kinematic_servo_range_trajectory): """ Sends a full servo value trajectory of discrete steps, to the arm. :param kinematic_servo_range_trajectory: :return: True if succeeded. """ action_successful = False servo_mask = self.control_world_model[ "active_links_mask"] # servo mask for step in kinematic_servo_range_trajectory: for i in range(len(step)): if servo_mask[i]: servo_value = step[i] current_servo = self.control_world_model["servo_count"] - i url = self.control_world_model["base_put_url"].format( self.arm_url, current_servo, servo_value) if self.control_world_model["verbose"]: print(url) try: r = requests.put(url, data="") if r.status_code != 200: break except Exception as e: print("Exception: {}".format(str(e))) time.sleep(self.control_world_model["command_delay"]) if self.control_world_model["verbose"]: print("") action_successful = True return action_successful def move_arm(self, target_position, last_servo_locations, trajectory_steps=-1): """ Gradually moves the end-effector of the robotic arm, from the latest known servo positions, to a desired 3D centimeter cartesian position. :param target_position: List of 3 values, the desired end-effector, 3D centimeter cartesian position. :param last_servo_locations: List of the latest 6 servo values in [500, 2500]. :param trajectory_steps: Scalar integer, the total steps for the end effector trajectory. :return: True if successfully move the arm. """ action_successful = False last_servo_values = self.init_position if trajectory_steps == -1: trajectory_steps = self.control_world_model["trajectory_steps"] # TODO: move last position to world model if self.control_world_model[ "detect_last_position"]: # TODO: get last servo values, world may be old...? last_servo_values = last_servo_locations try: if self.control_world_model["send_requests"]: url = "http://{}/".format(self.arm_url) r = requests.get(url, data="") if r.status_code == 200: result = r.json()["variables"] last_servo_values = np.array([ result["servo6"], result["servo5"], result["servo4"], result["servo3"], result["servo2"], result["servo1"] ]) if self.control_world_model["verbose"]: print("last_servo_values: ", last_servo_values) print( "last_servo_xyz", np.round( self.servo_range_to_xyz( last_servo_values, self.control_world_model[ "current_servo_monotony"]), 2)) except Exception as e_pos: print("Exception: {}".format(str(e_pos))) if self.control_world_model["center_init"]: if self.control_world_model["verbose"]: print( "Top position (radians): ", self.le_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix( self.init_position, np.eye(3)))) if self.control_world_model["show_plots"]: ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') self.le_arm_chain.plot(self.le_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix( self.init_position, np.eye(3))), ax, target=self.init_position) matplotlib.pyplot.show() init_position2 = self.init_position init_angle_radians2 = self.le_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix(init_position2, np.eye(3))) from_servo_range = self.radians_to_servo_range(init_angle_radians2) if self.control_world_model["detect_last_position"]: from_servo_range = last_servo_values to_servo_range = self.xyz_to_servo_range( target_position, self.control_world_model["current_servo_monotony"]) kinematic_servo_range_trajectory = self.get_servo_range_trajectory( from_servo_range, to_servo_range, trajectory_steps) if self.control_world_model["verbose"]: print( "init_angle_radians2: {}, from_servo_range: {}, to_servo_range: {}, servo_range_trajectory: {}" .format(init_angle_radians2, from_servo_range, to_servo_range, kinematic_servo_range_trajectory)) if self.control_world_model["show_plots"]: ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') self.le_arm_chain.plot(self.le_arm_chain.inverse_kinematics( geometry_utils.to_transformation_matrix( target_position, np.eye(3))), ax, target=target_position) matplotlib.pyplot.show() if self.control_world_model["send_requests"]: action_successful = self.send_restful_trajectory_requests( kinematic_servo_range_trajectory) return action_successful
# Get the arm and target nodes. target = supervisor.getFromDef('TARGET') arm = supervisor.getSelf() # Loop 1: Draw a circle on the paper sheet. print('Draw a circle on the paper sheet...') while supervisor.step(timeStep) != -1: t = supervisor.getTime() # Use the circle equation relatively to the arm base as an input of the IK algorithm. x = 0.25 * math.cos(t) + 1.1 y = 0.25 * math.sin(t) - 0.95 z = 0.23 # Call "ikpy" to compute the inverse kinematics of the arm. ikResults = armChain.inverse_kinematics([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]]) # Actuate the 3 first arm motors with the IK results. for i in range(3): motors[i].setPosition(ikResults[i + 1]) # Keep the hand orientation down. motors[4].setPosition(-ikResults[2] - ikResults[3] + math.pi / 2) # Keep the hand orientation perpendicular. motors[5].setPosition(ikResults[1]) # Conditions to start/stop drawing and leave this loop. if supervisor.getTime() > 2 * math.pi + 1.5: break elif supervisor.getTime() > 1.5: # Note: start to draw at 1.5 second to be sure the arm is well located. supervisor.getPen('pen').write(True)
# Get the arm and target nodes. target = supervisor.getFromDef('TARGET') arm = supervisor.getSelf() # add camera print('start find work space') # Loop 2: Move the arm hand to the target. print('Move the yellow and black sphere to move the arm...') while supervisor.step(timeStep) != -1: # Get the absolute postion of the target and the arm base. targetPosition = target.getPosition() armPosition = arm.getPosition() print(targetPosition) # Compute the position of the target relatively to the arm. # x and y axis are inverted because the arm is not aligned with the Webots global axes. x = targetPosition[0] - armPosition[0] y = -(targetPosition[2] - armPosition[2]) z = targetPosition[1] - armPosition[1] + 0.5 # Call "ikpy" to compute the inverse kinematics of the arm. ikResults = armChain.inverse_kinematics([x, y, z]) # Actuate the 3 first arm motors with the IK results. for i in range(3): motors[i].setPosition(ikResults[i + 1]) # y +- 0.7 # x +- 0.3
# print("init_servo_radians: ", init_servo_radians) # # print("init_servo_radians: ", np.multiply(servo_range_to_radians(init_servo_values), # current_servo_monotony[::-1])) # # init_position2 = le_arm_chain.forward_kinematics(init_servo_radians) # init_position = np.round(servo_range_to_xyz2(init_servo_values, current_servo_monotony), 2) # print("predicted_init_position: ", init_position) # print("proper init_position: ", target_position) except Exception as e: print("Exception: {}".format(str(e))) if center_init: print("Top position (radians): ", le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix( init_position, np.eye(3)))) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') le_arm_chain.plot(le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix( init_position, np.eye(3))), ax, target=init_position) matplotlib.pyplot.show() print("Target angles (radians): ", le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix( target_position, np.eye(3)))) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') le_arm_chain.plot(le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix( target_position,
orientation=[0, 0, 0], rotation=[0, 0, 1], ), URDFLink( name="second_link", translation_vector=[1, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1], ) ]) ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d') target_vector = [1, 0, 0] target_frame = np.eye(4) target_frame[:3, 3] = target_vector target_frame print("The angles of each joints are : ", test_link.inverse_kinematics(target_frame)) real_frame = test_link.forward_kinematics( test_link.inverse_kinematics(target_frame)) print("--> Computed position vector : %s" % real_frame[:3, 3]) print("--> original position vector : %s" % target_frame[:3, 3]) ax = plot_utils.init_3d_figure() test_link.plot(test_link.inverse_kinematics(target_frame), ax, target=target_vector) matplotlib.pyplot.xlim(-1, 2) matplotlib.pyplot.ylim(-1, 2) matplotlib.pyplot.show()