# 3. ROS1-Robot Arm Advanced Control Manual ## 3.1 Homogeneous Coordinates Transformation ### 3.1.1 Introduction to Homogeneous Coordinates Homogeneous coordinate transformation serves as a fundamental mathematical tool extensively applied in various domains, including computer graphics, robotics, and computer vision. Its purpose is to illustrate the transformation relationships between coordinate systems, with the primary goal of simplifying intricate geometric transformations like translation, rotation, scaling, and projection. This simplification allows for a more concise representation and computation of these transformations. In a two-dimensional plane, a pair of coordinate values (x, y) is used to precisely denote a point's position. If the coordinates of a point before the transformation are (x, y), and after the transformation are (x\*, y\*), this transformation process can be succinctly expressed in the following matrix form:

The approach of expressing a two-dimensional vector through a three-dimensional vector, or more broadly, representing an n-dimensional vector with an (n+1)-dimensional vector, is known as homogeneous coordinate representation. There is a **'one-to-many'** relationship between ordinary coordinates and homogeneous coordinates: Ordinary coordinates x h -\> Homogeneous coordinates Homogeneous coordinates ÷ h -\> Ordinary coordinates When h = 1, the obtained homogeneous coordinates are called 'normalized coordinates' because the first n coordinates correspond to n-dimensional coordinates in the ordinary coordinate system. ### 3.1.2 Homogeneous Coordinates Function By adopting the homogeneous coordinate representation, it becomes possible to uniformly express a two-dimensional linear transformation in the following normalized form:

For a graphical object, its geometric relationships can be described using a vertex table, while its topological relationships can be captured using an edge table. Therefore, when it comes to the transformation of a graphical object, it ultimately boils down to altering the vertex table to effect the desired changes. ### 3.1.3 Transformation * **Translation Transformation** Translation involves moving point P along a straight path from one coordinate position to another.

The formula for homogeneous coordinates is as follows:

Translation is a rigid transformation that displaces an object without inducing deformation, ensuring that every point on the object moves by an equal amount of coordinates. * **Ratio Transformation**

The formula for homogeneous coordinates is as follows:

The scaling factors Sx and Sy can take on any positive integer values. If the values are less than 1, the object's size will decrease. If the values exceed 1, the object will enlarge. Setting both values to 1 will maintain the object's original size. (1) Sx = Sy ratio:

(2) Sx \<\> Sy ratio:

When Sx = Sy, the scaling transformation becomes a uniform scaling transformation, calculated using the following matrix:

* **Symmetrical Transformation** Symmetric transformation, alternatively referred to as reflection or mirror transformation, produces an image that mirrors the original shape across a specific axis or the origin.

* **Rotation Transformation** Two-dimensional rotation refers to the process of repositioning point P by rotating it around the coordinate origin at a certain angle θ (counterclockwise as positive, clockwise as negative) to obtain a new point P\*. The transformation equation for the rotation of point P is initially determined with the origin as the reference point.

## 3.2 DH Modeling ### 3.2.1 Introduction to DH Modeling * **Introduction** The DH modeling method is an approach for low-degree mechanism modeling based on homogeneous transformation matrices. It is commonly employed to describe the geometric relationships and kinematics between joints in robotic arms. DH modeling stands out as one of the standard methods in the field of mechanical engineering for establishing kinematic models of robotic arms. This modeling technique utilizes a set of parameters to illustrate the relative positions and motions between the joints of a robotic arm, facilitating calculations for both forward and inverse kinematics. The fundamental concept behind DH modeling is to treat each joint of the robotic arm as a continuous rigid body undergoing rotation or translation. Subsequently, a series of rotations and translations are utilized to simulate the overall motion of the entire robotic arm. * **Function** DH modeling is predominantly employed in the field of robot kinematics. This approach involves creating a coordinate system on each link and employing homogeneous coordinate transformations to facilitate the conversion of coordinates between two links. In systems with multiple interconnected links, the iterative use of homogeneous coordinate transformations enables the establishment of the relationship between the initial and final coordinate systems. * **DH Modeling Operations** (1) List

(2) Modeling ① Set the initial position and designate labels for the links and joints. Generally, the initial position of a robotic arm is assumed to be fully extended upward, with the lateral joints oriented along the x-axis. The diagram on the right in the following image provides a visual representation of the link structure.

② Verify the axis lines of each joint and the z-axis. The axis lines represent the axes along which each joint rotates or moves. The z-axis corresponds to the current axis line, with an arbitrary direction (in this case, opposite to the motor rotation direction is chosen). The notation for the subscripts may differ between the two methods, so it's important to pay attention to the sign of the subscripts. The subscript on the z-axis indicates which coordinate system it belongs to.

(3) Set Coordinate System for Each Joint Begin by locating the origin of the {i} coordinate system and determining the x_i axis. In SDH, if the axis lines of {i} and {i-1} intersect, the intersection point becomes the origin. The x_i axis lies on the perpendicular bisector of the plane formed by the two axis lines, with an arbitrary direction. If the axis lines do not intersect, the origin is set at the point of intersection between the common perpendicular and the current joint axis {i}. The x_i axis then lies along this common perpendicular, with an arbitrary direction. In MDH, if the axis lines of {i} and {i+1} intersect, the intersection point becomes the origin, and the x_i axis lies on the perpendicular bisector of the plane formed by the two axis lines, with an arbitrary direction. If the axis lines do not intersect, the origin is set at the point of intersection between the common perpendicular and the current joint axis {i}. The x_i axis then lies along this common perpendicular, with an arbitrary direction. As there can be numerous parallel axis lines and an infinite number of common perpendiculars, and the directions of x and z axes can be arbitrarily set, this results in many possible versions of the parameter model. (4) Adding Initial and Final Coordinate Systems In SDH, {0} is already designated, so {6} is added at the end. In MDH, {6} is already designated, so {0} is added at the beginning. The setting of coordinate systems at the initial and final ends depends on practical considerations and personal preferences. The following describes the process of establishing coordinate systems.

:::{Note} Given the substantial flexibility in selecting the z-axis, x-axis, and origin, the method outlined above represents just one of several possible approaches to establishing coordinate systems. Decisions regarding how to establish coordinate systems should be guided by individual circumstances and considerations. ::: (5) Determining the 4 D-H Parameters Refer to the table generated in the first step and fill in the values one by one. Initially, do not concern yourself with the subscripts on the parameters as it may lead to confusion. Both the SDH and MDH methods involve coordinate transformations from the preceding coordinate system to the next one. When writing the ith row, focus on the relationship between {i-1} and {i}. However, there is a difference in the order of parameters. Here, we will use the example of the second row in the table. When i=2, we perform the coordinate transformation from {1} to {2}. The order for SDH is θ → d → α → a. Rotate the x\_{i-1} axis of coordinate system {i-1} about the z\_{i} axis to align with the x\_{i} axis. The rotation angle is θ\_{i}, with the right-hand rule defining the positive direction as the z-axis. ① Translate the x\_{i-1} axis along the z\_{i-1} axis to the x\_{i} axis by a distance of d\_{i}. ② Using the result of the previous transformation, rotate the z\_{i-1} axis of coordinate system {i-1} about the x\_{i-1} axis to align with the z\_{i} axis. The rotation angle is α\_{i}, with the right-hand rule defining the positive direction as the x-axis. ③ Translate the z\_{i-1} axis along the x\_{i-1} axis to the z\_{i} axis by a distance of a\_{i} until the two coordinate systems coincide.

Following the rules of SDH, the table below can be derived. In the diagram above, theta_2 is set to -90, but since it is a variable parameter, we designate it as q2.

The order for MDH is α → a → θ → d. ① Rotate the z\_{i-1} axis of coordinate system {i-1} about the x\_{i} axis to align with the z\_{i} axis. The rotation angle is α\_{i-1}, with the right-hand rule defining the positive direction as the z-axis. ② Translate the z\_{i-1} axis along the x\_{i-1} axis to the z\_{i} axis by a distance of a\_{i-1}. ③ Using the result of the previous transformation, rotate the x\_{i-1} axis of coordinate system {i-1} about the z\_{i-1} axis to align with the x\_{i} axis. The rotation angle is θ\_{i}, with the right-hand rule defining the positive direction as the x-axis. ④ Translate the x\_{i-1} axis along the z\_{i-1} axis to the x\_{i} axis by a distance of d\_{i}, until the two coordinate systems coincide.

Applying MDH rules, the table below can be generated. In the diagram above, theta_2 is assigned as 90, but as it is a variable parameter, we represent it as q2.

(6) Complete Table Fill in the table based on the DH method model and engineering drawings. (The official two-dimensional drawings and three-dimensional model dimensions are inconsistent. Since the primary use is the three-dimensional model, I measured the three-dimensional drawings again myself.)

### 3.2.2 JetArm DH Model (1) JetArm DH coordinate system：

(2) DH Model List: Please refer to the file below: [/home/jetarm/src/jetarm_driver/jetarm_kinematics/src/jetarm_kinematics/jetarm_6dof_params.py](../_static/source_code/jetarm_kinematics.zip) | i | αi-1 | ai-1 | di | θi | Θ Range | |:---:|:----:|:-------------:|:---:|:---:|:-------------:| | 1 | 0 | 0 | 0 | θ1 | θ1(-120, 120) | | 2 | -90 | 0 | 0 | θ2 | θ2(-180, 0) | | 3 | 0 | 0.12941763737 | 0 | θ3 | θ3(-120, 120) | | 4 | 0 | 0.12941763737 | 0 | θ4 | θ4(-200, 20) | | 5 | -90 | 0 | 0 | θ5 | θ5(-120, 120) | ## 3.3 Forward Kinematics Analysis ### 3.3.1 Introduction to Inverse Kinematics * **Introduction** Forward kinematics refers to the process of deriving the motion of the end effector from the joint movements of a robot. It involves calculating the position and orientation information of the end effector based on the joint coordinates of the robot. Forward kinematics is easier to understand and implement because it directly calculates the position and orientation of the end effector from the robot's joint movements. In robot control, forward kinematics is widely used in trajectory planning, simulation, visualization, and other aspects. * **Forward Kinematics Logic** The forward kinematics of a robotic arm is typically expressed in the following functional form:

This equation indicates that the pose of the end effector is a function based on the joint coordinates. If expressed using homogeneous transformations, the expression is a simple product of individual link transformation matrices given by Equation (2). For N links in a robotic arm, the expression becomes:

For any robotic arm, regardless of the number and configuration of its joints, the forward kinematics solution can be calculated. The practical task space for most robotic arms is typically three-dimensional. Controlling the motion of a robotic arm involves providing specific voltage control signals to each joint, causing them to move to specific angles and achieve the desired pose. For a 6-axis robotic arm, the overall matrix transformation is often denoted as T6. ### 3.3.2 JetArm Forward Kinematics Analysis | i | αi-1 | ai-1 | di | θi | Θ Range | | :--: | :---: | :-----------: | :--: | :--: | :-----------: | | 1 | 0 | 0 | 0 | θ1 | θ1(-120, 120) | | 2 | -90 | 0 | 0 | θ2 | θ2(-180, 0) | | 3 | 0 | 0.12941763737 | 0 | θ3 | θ3(-120, 120) | | 4 | 0 | 0.12941763737 | 0 | θ4 | θ4(-200, 20) | | 5 | -90 | 0 | 0 | θ5 | θ5(-120, 120) | The homogeneous transformation matrix form of DH is as follows:

Obtain the homogeneous transformation matrix for a single joint of JetArm: T(0-1)=\[\[Cθ1，-Sθ1，0，0\],\[Sθ1，Cθ1，0，0\],\[0，0，1，0\],\[0，0，0，1\]\] T(1-2)=\[\[Cθ2，0，Sθ2，0\],\[0，1，0，0\],\[-Sθ2，0，Cθ2，0\],\[0，0，0，1\]\] T(2-3)=\[\[Cθ3，-Sθ3，0，0\],\[Sθ3，Cθ3，0，0\],\[0，0，1，0\],\[0，0，0，1\]\] T(3-4)=\[\[Cθ4，-Sθ4，0，0\],\[Sθ4，Cθ4，0，0\],\[0，0，1，0\],\[0，0，0，1\]\] T(4-5)=\[\[Cθ5，0，Sθ5，0\],\[0，1，0，0\],\[-Sθ5，0，Cθ5，0\],\[0，0，0，1\]\] Multiply the homogeneous coordinates of individual joints to obtain the forward kinematics formula for JetArm: T(0-5)=T(0-1)\*T(1-2)\*T(2-3)\*T(3-4)\*T(4-5) Due to the fact that the servos on joints 2, 3, and 4 of the robotic arm are all horizontally rotating, T(1-2), T(2-3), and T(3-4) can be simplified using the sum and difference formulas: T(1-4) = T(1-2)\*T(2-3)\*T(3-4)=\[\[Cθ₂₃₄，-Sθ₂₃₄，0，Cθ₂₃a₃+Cθ₂a₂\], \[ 0， 0， 1，0\], \[-Sθ₂₃₄，-Cθ₂₃₄，0，-Sθ₂₃a₃-Sθ₂a₂\], \[ 0， 0， 0， 1\]\] Cθ₂₃₄ =cos(θ₂+θ₃+θ₄) Sθ₂₃₄=sin(θ₂+θ₃+θ₄) According to the geometric relationship between the robot arm and joint, the following can be obtained. T(1-4) =\[\[1，0，0，Cθ₂₃a₃+Cθ₂a₂\], \[ 0， 0， 1，0\], \[0，-1，0，-Sθ₂₃a₃-Sθ₂a₂\], \[ 0， 0， 0， 1\]\] The robotic arm kinematic formula is obtained as follows: T(0-5)=\[ \[Cθ₁Cθ₅+Sθ₁Sθ₅，-Cθ₁Sθ₅+Sθ₁Cθ₅，0，Cθ1(Cθ₂₃a₃+Cθ₂a₂+a₄)\], \[Sθ₁Cθ₅-Cθ₁Sθ₅，-Sθ₁Sθ₅-Cθ₁Cθ₅， 1，Sθ1(Cθ₂₃a₃+Cθ₂a₂+a₄)\], \[ 0， 0， -1，-d₅-Sθ₂₃a₃-Sθ₂a₂\], \[ 0， 0， 0， 1\]\] ### 3.3.3 Introduction to Forward Kinematics Service * **Preparation** (1) Assemble the robot arm according to [1. Getting Ready(JetArm User Manual)->1.2 Hardware Installation and Guidelines](1.Getting_Ready.md#hardware-installation-and-guidelines). (2) Start the robot arm according to [1. Getting Ready(JetArm User Manual) ->1.3 Initial Startup Instructions](1.Getting_Ready.md#initial-startup-instructions). * **Check Kinematics Service** (1) Click-on

to open the command-line terminal, and execute the command below to disable the app auto-start service. ``` sudo systemctl stop jetarm_bringup.service ``` (2) Execute the following command to initiate the robot arm SDK file. ``` roslaunch jetarm_sdk sdk_node_6dof.launch ``` (3) Open a new command-line terminal

, and run the command to launch the robot arm kinematics file. ``` roslaunch jetarm_kinematics kinematics_node_6dof.launch ``` (4) Open a new terminal, and type the command below to access the robot arm kinematics service. ``` rosservice list ```

, and execute the command below. ``` rosservice call /kinematics/set_joint_value_target "joint_value: [500.0,500.0,500.0,500.0,500.0]" ``` By inputting the rotation angles of servos 1-5, you can obtain the pose information of the robotic arm after movement. The information includes coordinates along the XYZ axes and the orientation represented using quaternions. :::{Note} The service call here only displays the pose information; the robotic arm does not physically move. :::

* **Programe outcome**

* **Launch File Control** (1) Press "**Ctrl+C**" to close the launch file on the terminal where robot arm SDK file and kinematics file are executed. (2) Subsequently, input the command below in

to initiate the forward kinematics service invocation program. You will witness the robotic arm calculating and maneuvering to the target position as per the configured servo pulse widths. Pertinent information will be presented in the terminal. ``` roslaunch jetarm_example fk.launch ```

(3) If you want to terminate the program, press '**Ctrl+C**'. If the program fails to stop, please have retry. * **Launch App Auto-Start Service** (1) Execute the command below on the terminal, and hit Enter to initiate the app service. Then input the password 'hiwonder'. ``` sudo systemctl start start_app_node.service ``` (2) After the app auto-start service is launched, the robot will restore initial pose and the buzzer will emit a 'beep' sound. * **Program Analysis** The source code is stored in [/home/hiwonder/jetarm/src/jetarm_example/src/kinematics_demo/fk.py](../_static/source_code/kinematics_demo.zip) The program's logical flowchart is illustrated in the diagram below.

As shown in the diagram, the program's logic is mainly divided into two parts: obtaining forward kinematics and calculating pulse width values. Following this, the calculated pulse width values are sent to the servo control function. (1) Import Feature Package Import the necessary module using the import statement. {lineno-start=4} ``` import rospy import signal import jetarm_kinematics.transform as transform from jetarm_kinematics.forward_kinematics import ForwardKinematics from jetarm_kinematics.inverse_kinematics import get_ik, get_position_ik, set_link, get_link, set_joint_range, get_joint_range from hiwonder_interfaces.msg import SerialServoMove from jetarm_sdk import bus_servo_control from hiwonder_interfaces.msg import MultiRawIdPosDur ``` The rospy library is the Python client library for ROS, used to write ROS nodes, publish and subscribe to messages, perform service calls, etc. Signal is a standard library in Python used for handling signals. The transform package provides functions necessary for kinematic coordinate system transformations. Import functions and classes required for forward and inverse kinematics calculations. hiwonder_interfaces.msg is a package containing message types, including SerialServoMove and MultiRawIdPosDur for the robotic arm's serial communication. Import the servo control module from the SDK library. (2) Initialize Nodes {lineno-start=14} ``` rospy.init_node('fk_demo', anonymous=True) #初始化节点(initialization node) ``` Create an object named fk for forward kinematics calculations. This object will calculate the position of the robot's end effector. {lineno-start=15} ``` fk = ForwardKinematics(debug=True) ``` (3) Set the Information Print on Terminal Print the servo pulse width value on the terminal. {lineno-start=16} ``` print("舵机脉宽值：") servo_list = [500,400,300,400,500] print(servo_list) ``` Print the radian value on the terminal. {lineno-start=20} ``` print("舵机脉宽值转为弧度：") pulse = transform.pulse2angle(servo_list) # 舵机脉宽值转为弧度(convert servo pulse width value to radians) print(pulse) ``` (4) Obtain the Forward Kinematics Solution Display the forward kinematics solution in the form of coordinate systems, quaternions, and Euler angles. {lineno-start=24} ``` res = fk.get_fk(pulse) #获取运动学正解(get kinematics solution) print('正运动学解-坐标：', res[0]) print('正运动学解-四元数：', res[1]) print('转换四元数成rpy：', transform.qua2rpy(res[1])) ``` Set servo data detection and servo ID detection to False. {lineno-start=29} ``` bus_servo_data_detection = False bus_servo_id_detection = False ``` (5) bus_servo_data_callback Function Set servo data detection and servo ID detection to False. Define global variables bus_servo_data_detection and j. If servo_id is equal to the value of the global variable j, increment the value of j. If servo_id is equal to 5, set the global variable bus_servo_data_detection to True. {lineno-start=32} ``` # 总线舵机数据回调函数(bus servo data callback function) def bus_servo_data_callback(msg): global bus_servo_data_detection,j #print(msg) if msg.servo_id == j: j+=1 if msg.servo_id == 5: #判断该话题的ID是否为空(determine if the topic ID is empty) bus_servo_data_detection = True ``` (6) Servo Motion Control Function The function bus_servo_controls(id=0, position=0, duration=0.0) is used to control the bus servo's movement. It accepts three parameters: "**id**" represents the servo's ID, numbered from 1 to 6. "**position**" represents the servo's angular position, ranging from 0 to 1000. "**duration**" represents the servo's operating time in milliseconds. {lineno-start=40} ``` def bus_servo_controls(id=0, position=0, duration=0.0): #bus_servo_data =[] # 设置总线舵机消息类型(set bus servo information type) data = SerialServoMove() data.servo_id = id #总线舵机ID (bus servo ID) data.position = position #总线舵机角度[0-1000](bus servo angle[0-1000]) data.duration = duration #总线舵机运行时间(bus servo running time) bus_servo_pub.publish(data) #发布数据(publish data) ``` (7) Publish and Receive Serial Bus Servo Topic ① Define `self.bus_servo_pub` to publish the bus servo topic. Use `rospy.Subscriber` to create an image subscriber with the following parameters: The first parameter `/jetarm_sdk/serial_servo/move` indicates the name of the bus servo topic. The second parameter `SerialServoMove` specifies the message type. The third parameter `queue_size=1` specifies the size of the message queue. ② Define `self.bus_servo_pub` to publish the bus servo topic. Use `rospy.Subscriber` to create an image subscriber with the following parameters: The first parameter `/jetarm_sdk/serial_servo/move` indicates the name of the bus servo topic. The second parameter `SerialServoMove` specifies the message type. The third parameter indicates calling the function `self.bus_servo_data_callback` to handle the received messages. {lineno-start=52} ``` # 发布总线舵机话题(publish bus servo topic) bus_servo_pub = rospy.Publisher('/jetarm_sdk/serial_servo/move', SerialServoMove, queue_size=1) # 接收总线舵机话题(receive bus servo topic) bus_servo_sub = rospy.Subscriber('/jetarm_sdk/serial_servo/move', SerialServoMove, bus_servo_data_callback) ``` Enter the while loop, and sequentially send the servo pulse width values obtained from forward kinematics to the corresponding servos, controlling the robotic arm to move to the target position. {lineno-start=56} ``` while True: rospy.wait_for_service('/kinematics/set_pose_target') bus_servo_controls(id =j,position =servo_list[j-1],duration=500) #发布数据(publish data) rospy.sleep(0.25) if bus_servo_data_detection: break ``` ## 3.4 Inverse Kinematics Analysis ### 3.4.1 Introduction Inverse kinematics is the process of determining the parameters of the joints that need to be set to achieve a desired pose. The inverse kinematics problem of a robotic arm is a crucial foundation for trajectory planning and control. The speed and accuracy of inverse kinematics solutions directly impact the precision of robotic arm trajectory planning and control. Therefore, for a six-degree-of-freedom robotic arm, designing a fast and accurate inverse kinematics solution is of utmost importance. ### 3.4.2 Analysis For a robotic arm, inverse kinematics involves determining the rotation angles of each joint given the position and orientation of the gripper. The three-dimensional motion of a robotic arm can be quite complex. To simplify the model, we eliminate the rotation joint at the base, allowing us to perform kinematic analysis in a two-dimensional plane. Inverse kinematic analysis typically involves extensive matrix computations, and the process is complex with significant computational requirements, making implementation challenging. To better suit our needs, we use a geometric approach to analyze the robotic arm.

We simplify the model of the robotic arm by removing the base pan-tilt and the end effector, focusing on the main body of the arm. From the diagram, we can see the coordinates (x, y) of the endpoint P of the robotic arm. Ultimately, it is composed of three parts (x1 + x2 + x3, y1 + y2 + y3). In the diagram, θ1, θ2, and θ3 are the angles of the servos that we need to solve, and α is the angle between the gripper and the horizontal plane. From the diagram, it's evident that the top-down angle of the gripper α = θ1 + θ2 + θ3. Based on this observation, we can formulate the following equation:

The values of x and y are provided by the user, while l1, l2, and l3 represent the inherent mechanical properties of the robotic arm's structure. For ease of calculation, we will preprocess the known components for a holistic consideration:

Substituting m and n into the existing equation and simplifying further, we can obtain:

Through calculation, we have:

We observe that the above expression is the quadratic formula for a single variable, where:

With this information, we can determine the angles θ1 and, similarly, calculate θ2. Consequently, we can determine the angles for all three servos. By controlling the servos based on these angles, we can achieve control over the coordinate position. ### 3.4.3 JetArm Inverse Kinematics Analysis Solving for θ1: θ1 = arctan（P_y/P_x）={Cθ₁(Cθ₂₃a₃+Cθ₂a₂+a₄) / Sθ₁(Cθ₂₃a₃+Cθ₂a₂+a₄)} Solving for θ2: θ2 = arcsin（-（P_z+d₅）/√（（Cθ₃a₂）\*（Cθ₃a₂）+（ Sθ₃a₃）\*（ Sθ₃a₃））-arctan(Sθ₃a₃/Cθ₃a₃+a₂) Solving for θ3: θ3 = +-arccos((P_z²+P_x²+P_y²-2a₄(Cθ₁P_x+Sθ₁P_y)+2P_xd₅+a₄²+d₅²-a₂²-a₃²)/(2a₂a₃)) Solving for θ4: θ4 = -(θ2+θ3) Solving for θ5: θ5 = θ1-arctan（n_y/n_x） Inverse kinematics formula:

### 3.4.4 Introduction to Inverse Kinematics Service * **Preparation** (1) Assemble the robot arm according to the tutorial provided in [1. Getting Ready(JetArm User Manual)->1.2 Hardware Installation and Guidelines](1.Getting_Ready.md#hardware-installation-and-guidelines). (2) Start the robot arm according to the instructions provided in [1. Getting Ready(JetArm User Manual)->1.3 Initial Startup Instructions](1.Getting_Ready.md#initial-startup-instructions). * **View Kinematics Service** (1) Click-on

to open the command-line terminal. Execute the command below and hit Enter. Then type the password, **hiwonder**. ``` sudo systemctl stop jetarm_bringup.service ``` (2) Execute the command below to launch robot arm SDK file. ``` roslaunch jetarm_sdk sdk_node_6dof.launch ``` (3) Open a new command-line terminal

, and run the command below to launch the robot arm kinematics service. ``` roslaunch jetarm_kinematics kinematics_node_6dof.launch ``` (4) Open a new command line terminal, and execute the command 'rosservice list' to view the robot arm kinematics service. ``` rosservice list ```

### 3.4.5 Servo Control Topic * **Invoke Service** Open a new command line terminal

, and execute the command below and complement the command using Tab key. ``` rosservice call /kinematics/set_pose_target "position: ```

Enter the target position's xyz coordinates (must be of float type). The pitch represents the servo's pitch angle, with a range of (-180,180). The pitch_range denotes the angle range, also within (-180,180), and resolution adjusts the angle range, fixed at 1.0. :::{Note} If the set target position and angle cannot be successfully reached by the robotic arm, the corresponding servo information will not be displayed in the terminal. In this case, the service call only displays pose information, and the robotic arm does not move. :::

* **Launch File Control** (1) Press "**Ctrl+C**" to close the launch file on the terminal where robot arm SDK file and kinematics file are executed. (2) Subsequently, input the command to initiate the forward kinematics service invocation program. You will witness the robotic arm calculating and maneuvering to the target position as per the configured servo pulse widths. Pertinent information will be presented in the terminal. ```pycon roslaunch jetarm_example ik.launch ```

(3) If you want to terminate the program, press '**Ctrl+C**'. If the program fails to stop, please have retry. * **Launch App Auto-Start Service** (1) Execute the command '**sudo systemctl start jetarm_bringup.service**' on the terminal, and hit Enter to initiate the app service. Then input the password '**hiwonder**'. ``` sudo systemctl start start_app_node.service ``` (2) After the app auto-start service is launched, the robot will restore initial pose and the buzzer will emit a 'beep' sound. * **Program Outcome**

* **Program Analysis** The source code is saved in [/home/hiwonder/jetarm/src/jetarm_example/src/kinematics_demo/ik.py](../_static/source_code/kinematics_demo.zip) The program's logical flowchart is illustrated in the diagram below.

As shown in the diagram, the program's logic is mainly divided into two parts: obtaining inverse kinematics and calculating pulse width values. Following this, the calculated pulse width values are sent to the servo control function. (1) Import Feature Package Import the necessary module using the import statement. {lineno-start=4} ``` import rospy import signal import jetarm_kinematics.transform as transform from jetarm_kinematics.forward_kinematics import ForwardKinematics from jetarm_kinematics.inverse_kinematics import get_ik, get_position_ik, set_link, get_link, set_joint_range, get_joint_range from hiwonder_interfaces.msg import SerialServoMove from jetarm_sdk import bus_servo_control ``` The rospy library is the Python client library for ROS, used to write ROS nodes, publish and subscribe to messages, perform service calls, etc. Signal is a standard library in Python used for handling signals. The transform package provides functions necessary for kinematic coordinate system transformations. Import functions and classes required for forward and inverse kinematics calculations. hiwonder_interfaces.msg is a package containing message types, including SerialServoMove and MultiRawIdPosDur for the robotic arm's serial communication. Import the servo control module from the SDK library. (2) Initialize Mode {lineno-start=14} ``` rospy.init_node('ik_demo', anonymous=True) #初始化节点(initialization node) ``` (3) Set Terminal Printing Information Print the end coordinate on the terminal. {lineno-start=15} ``` print("末端坐标：") coordinate = [0.35,0.0,0.24] print(coordinate) ``` Create an empty list: `servo_list` {lineno-start=19} ``` servo_list = [] ``` (4) Obtain Kinematic Inverse Solutions Retrieve kinematic inverse solutions from `get_ik`, convert the solutions into servo pulse width values, and store these values in the `servo_list` array. {lineno-start=22} ``` res = get_ik(coordinate,0, [-180, 180]) #获取运动学逆解(get kinematics inverse solution) if res != []: for i in range(len(res)): print('rpy%s:'%(i + 1), res[i][1]) # 解对应的rpy值(derive the corresponding roll, pitch, and yaw (RPY) values) pulse = transform.angle2pulse(res[i][0]) # 转为舵机脉宽值(convert to servo pulse-width value) servo_list = pulse[0] for j in range(len(pulse)): print('舵机脉宽值%s:'%(j + 1), pulse[j]) ``` Set servo data detection and servo ID detection to False. {lineno-start=32} ``` bus_servo_data_detection = False ``` (5) bus_servo_data_callback Function Define global variables `bus_servo_data_detection` and `j`. If `servo_id` equals the value of the global variable `j`, increment the value of `j`. If `servo_id` equals 5, set the global variable `bus_servo_data_detection` to True. {lineno-start=34} ``` # 总线舵机数据回调函数(bus servo data callback function) def bus_servo_data_callback(msg): global bus_servo_data_detection,j #print(msg) if msg.servo_id == j: j+=1 if msg.servo_id == 5: #判断该话题的ID是否为空(determine if the topic ID is empty) bus_servo_data_detection = True ``` (6) Servo Motion Control Function The function `bus_servo_controls(id=0, position=0, duration=0.0)` is used to control the motion of bus servos. It accepts three parameters: `id` represents the servo's ID, numbered from 1 to 5. `position` indicates the servo's angular position, ranging from 0 to 1000. `duration` represents the servo's operation time, measured in milliseconds. {lineno-start=43} ``` def bus_servo_controls(id=0, position=0, duration=0.0): #bus_servo_data =[] # 设置总线舵机消息类型(set bus servo information type) data = SerialServoMove() data.servo_id = id #总线舵机ID (bus servo ID) data.position = position #总线舵机角度[0-1000] data.duration = duration #总线舵机运行时间(bus servo angle[0-1000]) bus_servo_pub.publish(data) #发布数据(publish data) ``` (7) Publish and Receive Serial Bus Servo Topic ① Define `self.bus_servo_pub` to publish the bus servo topic. Create an image subscriber using `rospy.Subscriber` with the following parameters: The first parameter `/jetarm_sdk/serial_servo/move` denotes the bus servo topic name. The second parameter is the message type `SerialServoMove`. The third parameter `queue_size=1` specifies the size of the message queue. ② Define `self.bus_servo_pub` to publish the bus servo topic. Create an image subscriber using `rospy.Subscriber` with the following parameters: The first parameter `/jetarm_sdk/serial_servo/move` denotes the bus servo topic name. The second parameter is the message type `SerialServoMove`. The third parameter specifies the invocation of the `self.bus_servo_data_callback` function to process the received messages. {lineno-start=55} ``` # 发布总线舵机话题(publish bus servo topic) bus_servo_pub = rospy.Publisher('/jetarm_sdk/serial_servo/move', SerialServoMove, queue_size=1) # 接收总线舵机话题(receive bus servo topic) bus_servo_sub = rospy.Subscriber('/jetarm_sdk/serial_servo/move', SerialServoMove, bus_servo_data_callback) ``` After entering the loop, sequentially transmit the servo pulse width values obtained through inverse kinematics to the respective servo, thereby controlling the movement of the robot arm to reach the target position. {lineno-start=59} ``` while True: rospy.wait_for_service('/kinematics/set_pose_target') if servo_list != []: bus_servo_controls(id =j,position =int(servo_list[j-1]),duration=500) #发布数据(publish data) rospy.sleep(0.25) if bus_servo_data_detection: break else: break ```