Robot control method and control system

Abstract

The invention discloses a robot control method and system, electronic equipment, a computer readable storage medium and a computer program product, and the control method comprises the steps: obtaining to-be-processed information, the to-be-processed information comprises first sphere representation information of a robot, second sphere representation information of a target object and information of a forward kinematics function of the robot; based on the to-be-processed information, the expected acceleration of each joint of the robot is obtained through a Riemannian motion strategy; generating a control instruction according to the expected acceleration; and sending the control instruction to the robot to control the robot to reach a specified position. According to the method, the surface contours of different areas of various objects are expressed in the form of spheres, accurate simulation of the motion and collision states of the various objects is achieved, meanwhile, accurate calculation of the real-time positions of the various objects is achieved, and by combining sphere expression with a Riemannian motion strategy, the real-time position of the various objects can be accurately calculated. Therefore, accurate control over the robot is achieved.

Description

Technical Field

[0001] This application relates to the field of computers. For example, it relates to a control method and control system for a robot, an electronic device, a computer-readable storage medium, a computer program product, etc. Background Art

[0002] As artificial neural networks play an increasingly important role in robot control and planning algorithms, faster and more user-friendly robot learning and training platforms are widely needed in various fields such as automobiles, industrial robots, domestic robots, or drones. Currently, the more widely used algorithm is the algorithm with a deep learning module. One of the latest software used in related fields is Nvidia's Isaac Sim (a robot simulation platform), which is a GPU (Graphics Processing Unit)-based comprehensive simulator equipped with various existing control algorithms, such as the RRT (Rapidly-exploring Random Tree) algorithm, the RPM (Random Pathway Model, an algorithm for robot path planning and obstacle avoidance) algorithm, etc. However, its core code is not publicly available, and its closed nature makes it difficult to implement and train customized algorithms on this platform. In addition to Isaac Sim, some other traditional robot simulation platforms also make it difficult to implement and train learning-based robot motion planning / control algorithms on the corresponding platforms due to the non-open source code of the platform or mainly being used to verify traditional algorithms.

[0003] In addition, for the existing control methods of robots, their control accuracy needs to be improved, and they are prone to falling into local optima. Summary of the Invention

[0004] Some embodiments of this application overcome the related defects existing in the existing control methods of robots, and provide a control method and control system for a robot, an electronic device, a computer-readable storage medium, and a computer program product.

[0005] Some embodiments provide a control method for a robot. The robot includes a base, at least one link, and a joint connecting the at least one link and the base. The at least one link includes an end effector located at the end of the robot. The control method includes:

[0006] Obtain information to be processed, where the information to be processed includes first sphere representation information of the robot, second sphere representation information of a target object, and information on the forward kinematic function of the robot;

[0007] Based on the information to be processed, obtain the expected acceleration of each joint of the robot through a Riemannian motion strategy;

[0008] Generate a control command according to the desired acceleration;

[0009] Send the control command to the robot to control the robot to reach the specified position.

[0010] Optionally, the information to be processed further includes third sphere representation information corresponding to the floor or wall.

[0011] Optionally, the information to be processed further includes at least one of the real-time operation parameter information of the robot, the target position information of the end effector, and the translation parameter of the base.

[0012] Optionally, the Riemannian motion strategy includes several subtasks, and the subtasks include at least one of the following:

[0013] Target attractor subtask, collision avoidance subtask, joint limit subtask, joint speed upper limit subtask, end effector attitude alignment subtask, end effector normal vector alignment subtask.

[0014] Optionally, the input parameters of the target attractor subtask include the second sphere representation information corresponding to the target position on the target object to be approached, the differentiable forward kinematic function of the robot, and the current joint angles and speeds of the robot;

[0015] The Riemannian metric and the acceleration value corresponding to the target attractor subtask are calculated according to the normalized distance or the squared distance between the target position and the current position of the end effector.

[0016] Optionally, the input parameters of the collision avoidance subtask include a second sphere set representing one or more target objects in the target scene, a first sphere set representing one or more robot components, the differentiable forward kinematic function of the robot, and the current joint angles and speeds of the robot;

[0017] The Riemannian metric and the acceleration value corresponding to the collision avoidance subtask are calculated from the sum of the soft collision cost functions for all sphere pairs, where the sum of the soft collision cost functions is calculated from the pairwise distances between the first spheres in the first sphere set and the second spheres in the second sphere set.

[0018] Optionally, the input parameters of the end effector attitude alignment subtask include the target world coordinates of the end effector, the differentiable forward kinematic function of the robot, and the current joint angles and speeds of the robot;

[0019] The Riemannian metric and acceleration value corresponding to the end - effector attitude alignment subtask are calculated based on the differences between the current normal and tangent vectors of the end - effector and the desired normal and tangent vectors.

[0020] Optionally, the control method further includes:

[0021] Based on the information to be processed, obtaining the additional acceleration of each joint of the robot through a pre - trained neural network;

[0022] The specific process of generating the control instruction according to the desired acceleration includes:

[0023] Superposing the desired acceleration and the additional acceleration to obtain the joint acceleration of each joint of the robot;

[0024] Generating a control instruction according to the joint acceleration.

[0025] Optionally, the calculation formula of the joint acceleration is as follows:

[0026] a joint =a base +w*a residual ;

[0027] where, a joint represents the joint acceleration, a base represents the desired acceleration, a residual represents the additional acceleration, and w represents a positive weight.

[0028] Optionally, the input of the neural network includes the distance vector from the robot to the obstacle, the three - dimensional pose difference vector between the current position and attitude of the end - effector and the target position and attitude, and a customized reward for stimulating good behavior; and / or,

[0029] The neural network is a permutation - invariant neural network.

[0030] Optionally, the training process of the neural network specifically includes:

[0031] Obtaining input parameters;

[0032] where, the input parameters include a second set of spheres representing one or more target objects in a static pose in the target scene, a first set of spheres representing one or more robot components in a static pose, the differentiable forward kinematic function of the robot, the current joint angles and speeds of the robot, the initial 6D poses of all target objects and the robot base, and the target world coordinates of the robot end - effector;

[0033] Calculating the positions of the first spheres corresponding to the robot and the second spheres corresponding to the target objects at the current moment;

[0034] Iteratively generate control instructions for the robot, and use the generated control instructions, as well as the current joint angles and speeds of the robot, to iteratively update the next set of joint angles and speeds of the robot at the next moment;

[0035] Iteratively update the relevant parameters of the neural network according to the simulation trajectory results of the robot until the training of the neural network is completed.

[0036] Optionally, the control method is implemented based on a GPU.

[0037] Optionally, the control method further includes:

[0038] Perform operations using a batch processing operator for parallel computing on the GPU; and / or,

[0039] Pre-compile and warm up the TensorFlow computation graph of the Riemannian motion strategy in the initialization phase, and use the pre-compiled TensorFlow computation graph to perform the calculation of joint acceleration during the reinforcement learning process of the Riemannian motion strategy.

[0040] Some embodiments further provide a control system for a robot, the control system includes: one or more processors, and the one or more processors are configured to execute a computer program to implement the foregoing control method for the robot.

[0041] Some embodiments further provide an electronic device, including a memory, a processor, and a computer program stored on the memory and configured to run on the processor, and when the processor executes the computer program, the foregoing control method for the robot is implemented.

[0042] Some embodiments further provide a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the foregoing control method for the robot is implemented.

[0043] Some embodiments further provide a computer program product, including a computer program, and when the computer program is executed by a processor, the foregoing control method for the robot is implemented.

[0044] Some embodiments further provide a computer program, and when the computer program is executed by one or more processors, the foregoing control method for the robot can be implemented.

[0045] The positive effects of some embodiments are as follows: A control method for a robot is provided. By representing the surface contours of different regions of various objects in the form of spheres, accurate simulation of the motion and collision states of various objects is achieved. At the same time, accurate calculation of the real-time positions of various objects is also achieved. Moreover, by combining the sphere representation with the Riemannian motion strategy, accurate control of the robot is realized. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 For the control method of the robot provided in Embodiment 1 of the present disclosure Process Schematic Diagram .

[0047] Figure 2 It is a schematic flowchart of a method for representing the surface contour of a target object provided in Embodiment 1 of the present disclosure.

[0048] Figure 3 It is a schematic diagram of a neighborhood provided in some embodiments of the present disclosure.

[0049] Figure 4 It is an overlapping image of the three-dimensional model of a teapot and the sphere used to represent the surface contour of the teapot in a static pose frame provided in some embodiments of the present disclosure.

[0050] Figure 5 It is a schematic diagram of voxel-based sampling provided in some embodiments of the present disclosure.

[0051] Figure 6 It is a schematic diagram of an example of the position in the world coordinate when the sphere used to represent the surface contour of an object is superimposed on the actual underlying physical object provided in some embodiments of the present disclosure.

[0052] Figure 7 It is a schematic flowchart of a method for obtaining the three-dimensional model of a target object provided in some embodiments of the present disclosure.

[0053] Figure 8 It is a schematic flowchart of a method for obtaining the three-dimensional model of a hand in real time provided in some embodiments of the present disclosure.

[0054] Figure 9 It is a schematic diagram of the mesh model of a hand provided in some embodiments of the present disclosure.

[0055] Figure 10 It is a schematic diagram of a system 800 for representing the surface contour of a target object provided in some embodiments of the present disclosure.

[0056] Figure 11 It is a schematic diagram of the architecture of a neural network provided in some embodiments of the present disclosure.

[0057] Figure 12Schematic diagram of the architecture of the GPU-accelerated digital twin reconstruction platform provided by some embodiments of the present disclosure.

[0058] Figure 13 Schematic diagram of the electronic device provided by Embodiment 3 of the present disclosure. Detailed implementation manners

[0059] The present invention will be further illustrated by way of embodiments below, but the present invention is not limited to the scope of the described embodiments thereby.

[0060] Referring to "embodiments" herein means that the specific features, structures or characteristics described in connection with the embodiments may be included in at least one embodiment of the present application. The phrase does not necessarily refer to the same embodiment everywhere in the text, nor is it an independent or alternative embodiment mutually exclusive with other embodiments. Those skilled in the art explicitly and implicitly understand that the embodiments described herein may be combined with other embodiments.

[0061] It should be understood that the "devices", "systems", "units" and / or "modules" used herein are a way to distinguish different components, elements, parts, portions or assemblies at different levels. However, if other words can achieve the same purpose, the said words may be replaced by other expressions.

[0062] As shown herein, unless the context clearly indicates an exception, words such as "a", "an", "one" and / or "the" are not specifically singular and may also include plural. Generally speaking, the terms "comprising" and "including" only indicate the inclusion of the clearly identified steps and elements, and these steps and elements do not constitute an exclusive list. The method or device may also include other steps or elements.

[0063] The definitions included herein, as used herein, the terms "having", "may have", "including" or "may include" indicate the existence of the corresponding functions, operations, elements, etc. herein, and do not limit the existence of one or more other functions, operations, elements, etc. In addition, it should be understood that, as used herein, the terms "including" or "having" indicate the existence of the features, numbers, steps, operations, elements, parts or combinations thereof described in the specification, and do not exclude the existence or addition of one or more other features, numbers, steps, operations, elements, parts or combinations thereof.

[0064] Flowcharts are used herein to illustrate the operations performed by the systems according to the embodiments herein. It should be understood that the operations before or after do not necessarily have to be executed precisely in order. On the contrary, the steps may be processed in reverse order or simultaneously. At the same time, other operations may also be added to these processes, or one or several steps may be removed from these processes.

[0065] In some examples of the present disclosure, the method steps described below can be implemented by a program, a custom circuit, or a combination of a custom circuit and a program. For example, the following method steps can be executed by a combination of one or more GPUs, one or more CPUs, or any other technically feasible one or more processors such as an ASIC chip. In some examples of the present disclosure, one or more processors such as an ASIC chip may include a memory. Further, those skilled in the art can understand that the protection scope of the present disclosure may include any system and device that can execute the following method steps.

[0066] Embodiment 1

[0067] The present disclosure provides a method for controlling a robot, where the robot includes a base, at least one link, and a joint connecting the at least one link and the base, and the at least one link includes an end effector located at the end of the robot, such as Figure 1 as shown, the control method includes:

[0068] S1. Obtain information to be processed, where the information to be processed includes the first spherical representation information of the robot, the second spherical representation information of the target object, and the information of the forward kinematic function of the robot;

[0069] S2. Based on the information to be processed, obtain the desired acceleration of each joint of the robot through a Riemannian motion strategy;

[0070] S3. Generate a control instruction according to the desired acceleration;

[0071] S4. Send the control instruction to the robot to control the robot to reach a specified position.

[0072] By representing the surface profiles of different regions of various objects in the form of spheres, the present disclosure achieves an accurate simulation of the motion and collision states of various objects, and also achieves an accurate calculation of the real-time positions of various objects. For this purpose, in an optional embodiment, the present disclosure provides a method for representing the surface profile of a target object, such as Figure 2 as shown, the method includes the following steps:

[0073] S101. Obtain a three-dimensional model of the target object.

[0074] S102. Based on the three-dimensional model, generate a preset number of spheres to represent the surface profiles of different regions of the target object.

[0075] Wherein, the density of the spheres in different regions corresponds to the geometric complexity of the surface profiles of different regions.

[0076] The inventors have found that although the use of simplified geometric primitives (such as spheres) to represent three-dimensional surfaces has found applications in applications that require collision detection, traditional techniques rely on uniform sampling or simple geometric approximations, and these techniques lack adaptability to local surface features that represent the geometric complexity of the surface profile, such as curvature and vertex density. This can lead to poor accuracy or excessive computational overhead when modeling complex geometric bodies.

[0077] In some embodiments of the present disclosure, a preset number of spheres are generated based on a three-dimensional model to represent the surface profiles of different regions of a target object, and the density of the spheres in different regions corresponds to the geometric complexity of the surface profiles of different regions, achieving accurate modeling of complex geometric bodies and at the same time balancing the computational overhead.

[0078] Graphical User Interface (GUI)

[0079] The graphical user interface (GUI) is an interface that allows users to easily and intuitively control the robot. The GUI will display the videos from each camera and the rendering results of the 3D scene. To control the robot, the user only needs to select a command from the toolbar and then click on the interactive video panel.

[0080] The inventors have found that the initial implementation of the current GUI visualization pipeline is inefficient, seriously affecting the frames per second (FPS) of the entire system, including tasks such as mask segmentation, pose estimation, and control processing. The main bottleneck lies in the inefficient data transfer between the CPU and the GPU, and frequent memory copying leads to latency peaks and performance degradation. The inventors have also found that the rendering pipeline runs in a synchronous blocking mode, preventing other processing threads from executing in parallel and causing further latency. Using an unoptimized GUI framework exacerbates these problems because it fails to utilize modern GPU acceleration techniques (such as direct GPU memory sharing), resulting in excessive CPU usage and slower rendering times for the scene mesh and surface spheres.

[0081] Using the spheres generated in some embodiments of the present disclosure to represent the surface contour of the target object can achieve the effect of optimizing the rendering pipeline. For example, memory sharing can be achieved using the GPU integrated with PyTorch (an open-source deep learning framework for machine learning and deep learning) - OpenGL (Open Graphics Library, an open graphics library and a cross-language, cross-platform application programming interface for rendering 2D and 3D vector graphics). This method realizes zero-copy data sharing between PyTorch tensors and OpenGL buffers, eliminates redundant memory transfers, and enables direct GPU rendering of visual outputs. The solutions provided in some embodiments of the present disclosure can implement custom OpenGL shaders, thereby enabling the direct processing of transformation and lighting effects on the GPU and reducing the computational burden on the CPU. The solutions provided in some embodiments of the present disclosure can separate the rendering tasks from data processing using parallel threads to ensure that GUI updates no longer impede critical system operations. In addition, the frame rate capping mechanism dynamically adjusts GUI updates according to the system load to maintain the stability of the FPS (frame rate). Further optimizations include the lazy loading and caching strategies of static meshes to reduce redundant calculations and memory usage. These enhancements together create a scalable, responsive, and real-time visualization system that can run seamlessly with other computationally intensive tasks.

[0082] In an alternative embodiment, the different regions include a first region and a second region, and the geometric complexity of the surface contour of the first region is greater than that of the second region; the density of the spheres in the different regions corresponding to the geometric complexity of the surface contours of the different regions includes: the density of the spheres in the first region is greater than that of the spheres in the second region.

[0083] In an alternative embodiment, the method further includes:

[0084] S103. Determine the values of the density weights of the alternative sampling points in different regions of the target object.

[0085] Among them, the value of the density weight represents the information of the geometric complexity of the surface contour of different regions.

[0086] In an alternative embodiment, step S103 includes:

[0087] S1031. Determine the neighborhood of the alternative sampling point based on the position of the alternative sampling point and the value of the preset neighborhood size.

[0088] S1032. Determine the value of the density weight of the alternative sampling point based on the neighborhood.

[0089] In an alternative embodiment, step S1032 includes: determining the value of the density weight of the alternative sampling point according to the average distance between the alternative sampling point and other alternative sampling points in the neighborhood.

[0090] Specifically, the value of the density weight of the alternative sampling point can be determined according to the following formula:

[0091] where w i is the density weight; is the average distance; ∈1 is a constant.

[0092] Specifically, ∈1 is a small constant, which functions to prevent the denominator of the above formula from being 0, resulting in unstable values of w i . It can be seen from the above formula that w i increases as decreases, because represents the average distance between the alternative sampling point and other alternative sampling points in the neighborhood, and decreases, that is, the distribution density of the alternative sampling points in the neighborhood increases. Therefore, the above formula assigns a higher density weight to the area of the alternative sampling point, ensuring that when determining the sampling point from the alternative sampling points, priority is given to determining the sampling point in the area with high geometric complexity of the surface contour.

[0093] Among them, the average distance

[0094]

[0095] can be determined by the following formula where is the average distance, i is the set of other vertices in the neighborhood, p j is the alternative sampling point serving as the neighborhood center; p

[0096]

[0097] In an alternative embodiment, the three-dimensional model is a polygon mesh model, where the polygon mesh model is composed of a set of vertices, edges and faces, and the alternative sampling points are the vertices of the polygon mesh model. Step S1031 includes:

[0098] Taking the alternative sampling point as the center, determining the range where the number of polygon edges separated from the alternative sampling point is no more than n as the neighborhood of the alternative sampling point, where the preset neighborhood size value is n and n is a positive integer.

[0099] Computer Graphics

[0100] Computer graphics is the field of computer-based image rendering techniques that generate photo-realistic two-dimensional images. In this field, three-dimensional objects are represented using a three-dimensional object model, which is typically composed of a point cloud triangulated mesh represented by the coordinates of vertices and triangular faces, in addition to a color texture map of the model. A virtual camera and lighting model are used to mathematically render the three-dimensional object into a corresponding 2D image. The rendering methods themselves range from computationally inexpensive methods (such as rasterization) to computationally expensive methods (such as ray tracing). In some embodiments of the present disclosure, rasterization-based image rendering techniques may be used.

[0101] Specifically, in computer graphics, polygonal mesh models define the shape and outline of each 3D object. Polygonal meshes are built from smaller interconnected planes (usually triangles or rectangles) that act like a 3D puzzle. Figure 1 Each vertex in a polygon mesh stores x, y, and z coordinate information. Each face of that polygon then contains surface information.

[0102] Neighborhood

[0103] A neighborhood with a preset neighborhood size of n can be called an n-ring neighborhood, which defines the area around a vertex based on the connectivity of adjacent faces. Figure 3 As shown, vertex p i The n-ring neighborhood of the vertex p i Starting from the grid edge, all vertices that can be reached within n traversal steps, for example, the 1-ring neighborhood consists of the vertices with the vertex p i The 2-ring neighborhood consists of directly connected vertices (sharing an edge), and the 2-ring neighborhood consists of the vertices with the vertex p i The number of sides between which the polygon is separated is not greater than 2.

[0104] The parameter n, which controls the size of the n-ring neighborhood, is configurable. Larger values of n expand the neighborhood, capturing a wider range of surface features, while smaller values of n reduce the neighborhood, focusing on local details.

[0105] The adjacency relationship of the n-ring neighborhood can be calculated using the sparse adjacency matrix A. The specific formula is as follows:

[0106]

[0107] The expansion of the neighborhood is achieved by iteratively multiplying the adjacency matrix. The specific formula is as follows:

[0108]

[0109] in, is a vertex in the k-ring neighborhood, Ak represents the k-th power of matrix A, δ i is a vector representing the initial vertex.

[0110] By changing the parameter n that controls the range size of the n-ring neighborhood, the analysis range can be set to adapt to local or global geometry. A smaller ring neighborhood confines the calculation to closer neighborhoods and preserves finer details, while a larger ring neighborhood encompasses a wider surface environment and can capture global structures at the cost of higher computational cost, thus enabling a balance between detail preservation and computational efficiency.

[0111] In an alternative embodiment, the three-dimensional model is a voxel model.

[0112] Among them, the voxel model is composed of a set of voxels, and the alternative sampling points are the centers of the outermost voxels of the voxel model.

[0113] Step S1031 includes:

[0114] Taking the alternative sampling point as the center, the range with a distance from the alternative sampling point not greater than r is determined as the neighborhood of the alternative sampling point.

[0115] Among them, the preset value of the neighborhood size is r, and r is a positive number.

[0116] Specifically, the voxel model simulates the surface geometry of the target object based on voxels and discretizes the 3D object into a structured grid of cubic cells (voxels). The resolution of the voxel model is controlled by the voxel_resolution parameter, which determines the number of divisions along the longest axis of the bounding box. Higher resolution can provide finer sampling but at the cost of increased computational complexity.

[0117] In some embodiments, a voxel model can be generated using a polygon mesh model. For example, the polygon mesh model can be converted into a voxel model before sampling. Considering that the voxels in the voxel model are usually very dense, in some embodiments, only some surface voxels (e.g., the outermost voxels) can be used for further sampling. In some embodiments, when the target object with a spherical surface contour is a flexible object (e.g., a human hand), the vertex indices of the polygon mesh model can be used for sampling without converting the polygon mesh model into a voxel model.

[0118] In an alternative embodiment, step S102 includes:

[0119] S1021. Determine a preset number of sampling points among all alternative sampling points based on the density weight value.

[0120] S1022. Generate a preset number of spheres with the determined sampling points as the centers.

[0121] In an alternative embodiment, step S1021 includes:

[0122] S10211. Determine one of the alternative sampling points as the initial sampling point.

[0123] S10212. Determine the next sampling point from the remaining alternative sampling points according to the value of the Euclidean distance between each remaining alternative sampling point and the alternative sampling point that was last determined as a sampling point, and the value of the density weight of each remaining alternative sampling point, until the predetermined number of sampling points is determined.

[0124] Wherein, the remaining alternative sampling points are the alternative sampling points that have not been determined as sampling points.

[0125] Density-Weighted Farthest Point Sampling

[0126] In an alternative embodiment, step S10212 includes: determining the value of the comprehensive distance between each remaining alternative sampling point and the alternative sampling point that was last determined as a sampling point according to the value of the Euclidean distance between each remaining alternative sampling point and the alternative sampling point that was last determined as a sampling point, and the value of the density weight of each remaining alternative sampling point; determining the remaining alternative sampling point with the largest corresponding comprehensive distance value as the next sampling point.

[0127] For example, the weighted distance can be determined according to the following formula:

[0128]

[0129] Wherein, d′(p i , p j ) is the comprehensive distance between the alternative sampling point p i and the previously determined sampling point p j , d(p i , p j ) is the Euclidean distance between the alternative sampling point p i and the previously determined sampling point p j , is the density weight of the alternative sampling point p i .

[0130] j next = max i (d′(p i , p j ))

[0131] The determined or selected next sampling point j next is the one with the maximum comprehensive distance value d′(p i , p jThe sampling point p of ( ) i 。

[0132] The aforementioned formula is the formula of the DWFPS (Density-Weighted Farthest Point Sampling) strategy. The DWFPS strategy iteratively selects the vertex farthest from the previously determined sphere center, and at the same time combines the density weight to bias the process of selecting the next sphere center towards the region with a higher vertex distribution density (i.e., the region with a more complex geometry). The DWFPS strategy ensures a higher sampling frequency in the region with a high vertex distribution density and a lower sampling frequency in the region with a low vertex distribution density. Therefore, this method achieves adaptive sampling, which can effectively capture fine details and large-scale structures without redundant calculations. The DWFPS strategy can ensure uniform spatial coverage and give priority to regions with higher complexity.

[0133] Different from the SDF (Signed Distance Function) method, the SDF method occupies a large amount of memory because it generates a surface distance field function suitable for any voxel in the 3D space of interest, while our sphere-based calculation is only performed pairwise between a small number of spheres represented as 3D point clouds. Correspondingly, our method occupies very little CPU / GPU memory, but is sufficient to generate a cost / reward function that can significantly change the robot's motion.

[0134] Adaptive Sphere Radius for Regional Surface Profile Variation

[0135] Optionally, in some embodiments of the present disclosure, the size of the sphere can also be adjusted accordingly according to the change of the surface contour of the target object. Figure 4 For example, the three-dimensional model of a teapot and the overlapping image of the sphere used to represent the surface contour of the teapot in the static pose frame are used to illustrate this process. Figure 4 (a) The figure shows the use of an adaptive sphere radius, which can be adjusted according to the geometric complexity of the local region. For comparison, Figure 4 (b) shows the representation method with a fixed sphere radius.

[0136] In an alternative embodiment, the method further includes:

[0137] S104. Based on the three-dimensional model, determine the sphere radii of a preset number of spheres.

[0138] Wherein, the sphere radii of the spheres in different regions correspond to the geometric complexity of the surface contours of different regions.

[0139] In an alternative embodiment, the sphere radius of the spheres in different regions corresponding to the geometric complexity of the surface profiles of different regions includes: the sphere radius of the spheres in the first region is smaller than the sphere radius of the spheres in the second region. For example, as Figure 4 (a) shows, the sphere radius in region 310 of the teapot is smaller than the sphere radius in region 312, and the sphere radius in region 314 of the teapot is smaller than the sphere radius in region 310.

[0140] In an alternative embodiment, the sampling points include a first sampling point and a second sampling point.

[0141] Step S104 may include:

[0142] S1041. Determine the sphere radius value of each sphere according to the preset radius value and the density weight value of the sampling points corresponding to each sphere.

[0143] Wherein, when the density weight value of the first sampling point is greater than the density weight value of the second sampling point, the sphere radius value of the sphere corresponding to the first sampling point is smaller than the sphere radius value of the sphere corresponding to the second sampling point.

[0144] In an alternative embodiment, the method further includes:

[0145] S105. Normalize the density weight value of the sampling points corresponding to each sphere to obtain the normalized density weight value of the sampling points corresponding to each sphere.

[0146] Wherein, the normalized density weight value is a positive number less than 1.

[0147] Specifically, to ensure that the density weight value remains bounded, the following formula can be used to normalize the density weight:

[0148]

[0149] Wherein, w i is the density weight, is the normalized density weight.

[0150] Through the above normalization process, the density weight value can be mapped to the range between 0 and 1, preventing extreme changes in the density weight value and ensuring the stability of the density weight value.

[0151] In an alternative embodiment, step S1041 includes:

[0152] S10411. Determine the sphere radius value of each sphere according to the preset radius value and the normalized density weight value of the sampling points corresponding to each sphere.

[0153] In an alternative embodiment, S10411 includes:

[0154] Determine the sphere radius value of each sphere according to the value of the preset radius, the value of the normalized density weight of the sampling points corresponding to each sphere, and the value of the proportionality coefficient.

[0155] Wherein, the proportionality coefficient is used to control the sensitivity of the change of the sphere radius value of each sphere to the value of the normalized density weight of the sampling points corresponding to each sphere.

[0156] Specifically, the sphere radius value can be determined according to the following formula:

[0157]

[0158] Wherein, r0 is the preset radius, which can be defined by the user, is the normalized density weight, and s is the proportionality coefficient.

[0159] The proportionality coefficient s is used to control the sensitivity of the change of the sphere radius to the value of the density weight. The exponential term e x ensures that the sphere radius r i can change smoothly and continuously without sudden jumps, so that the sphere can adapt to local geometric changes seamlessly. In some embodiments, the value of the proportionality coefficient s can be set to 10, and then the generated sphere is overlaid with the polygon mesh model of the target object to visually check the result. If the sphere is too small, the proportionality coefficient s can be increased. If the sphere is too large, the proportionality coefficient s can be decreased. In some embodiments, the value of the proportionality coefficient s can be 0.1, 1, 10, 100, 1000, or any value between 0.1 and 1000.

[0160] Through the above formula, the spheres representing the surface contours of different regions of the target object can shrink in the areas where the alternative sampling points are densely distributed, and expand in the areas where the alternative sampling points are sparsely distributed, so as to achieve adaptive coverage without introducing unnecessary computational costs.

[0161] Voxel-Based Sampling

[0162] In some embodiments, the three-dimensional model can be, for example, a voxel model. When the three-dimensional model is a voxel model, the density weight can also be normalized according to the above formula, and then the sphere radius can be determined according to the normalized density weight and the above formula. Since the sphere radius is scaled according to the local voxel density, it allows for finer details to be provided in high-density regions and larger spheres in sparse regions. In cases where uniformity is prioritized over adaptability, a fixed sphere radius can also be used instead of determining the sphere radius through the foregoing method.

[0163] Using a voxel model as a 3D model has the following several advantages over using a polygon mesh model as a 3D model:

[0164] 1. Works independently of mesh topology, allowing spheres to be placed outside the mesh to capture volumetric features and discontinuous regions.

[0165] 2. Provides uniform coverage, making it effective for sparse or low-resolution meshes.

[0166] Figure 5 A voxel-based sampling process is shown, in which the object is subdivided into a voxel grid and the spheres are positioned at the centers of the voxels using DWFPS.

[0167] In an alternative embodiment, the method further includes:

[0168] S106. Generate descriptor information for a preset number of spheres.

[0169] Wherein, the descriptor information includes at least one of index information, position information, normal information, and sphere radius. The index information is an index list, and the index list is a subset of the alternative sampling point index list {1, 2, 3, …, n IH}, and n IH is the total number of spheres representing the surface contour of the target object.

[0170] Specifically, this descriptor information can be seamlessly integrated with downstream tasks such as a collision detection system.

[0171] In an alternative embodiment, the method further includes:

[0172] S107. Obtain the position information of all alternative sampling points in real time.

[0173] S108. Determine the position information of all sampling points according to the position information and index information of all alternative sampling points.

[0174] Specifically, once a specific set of hand mesh vertex indices I H (i.e., integer indices i IH within the subset of the complete hand mesh vertex index {1, 2, 3, …, n H}, and n IH is the total number of hand mesh vertices) is used as the mounting points of the hand surface spheres (this set of indices only needs to be generated once and can be continuously reused), the positions of the hand surface spheres can be continuously calculated in real time. Represent the complete hand mesh vertices generated by the simpleHand model in the world coordinate system as V hand , and represent the center position of the hand surface sphere in the world coordinate system as S hand , then the position of S hand can be easily calculated at any time using the array index selection operation, and S =Vhand [I H 。

[0175] Based on the surface sphere representations generated for robotic components, rigid and flexible objects (such as a human hand) in the world coordinate system, they can be directly used for different types of real-time robotic control operations. Figure 6 Shows an example of the position in world coordinates when a sphere representing the surface contour of an object is superimposed on the actual underlying physical object. Figure 6 All the spheres representing the surface contours of rigid objects, robotic arms, and human hands that are generated in real time are shown in the scene. Figure 6 (a) Shows a three-dimensional scene including various rigid objects, robotic arm 510, and human hand model 520. Figure 6 (b) Shows the superimposition of the scene with sphere 550 and the normal vector 560 of the sphere.

[0176] Optionally, in some embodiments, a method S200 for obtaining a three-dimensional model of a target object is provided as Figure 7 shown, and this method S200 includes:

[0177] S210. Obtain the data of the real-time video stream. For example, obtain the real-time video stream through camera acquisition. There is a two-dimensional image of the target object in the video stream, so that the two-dimensional image of the target object can be acquired in real time.

[0178] S220. Generate a three-dimensional model of the target object based on the two-dimensional image of the target object.

[0179] Those skilled in the art can understand that there are different ways to generate a three-dimensional model of the target object based on the two-dimensional image of the target object. For example, in some embodiments, the following method can be adopted to generate the three-dimensional model.

[0180] S222. Determine the bounding box of the target object in the two-dimensional image. The bounding box of the target object in the two-dimensional image can be determined based on an open-vocabulary object detection model and the input information corresponding to the target object. For example, the two-dimensional image can be input into an open-vocabulary (such as "hand") object detection model. Through the vocabulary corresponding to the target object, the target object is recognized in the two-dimensional image, and the position and size of the target object in the two-dimensional image are determined. According to the position and size, a bounding box is generated in the two-dimensional image to enclose the target object within the bounding box. The bounding box can indicate the position and size of the target object in the two-dimensional image.

[0181] S224. Generate a three-dimensional model of the target object based on the two-dimensional image and the bounding box of the target object.

[0182] In some embodiments, the target object can be, for example, a hand. Now, an example will be given with the target object being a hand.

[0183] In some embodiments, step S224 may include:

[0184] S226. Determine the 2D hand joints and 3D hand vertex positions of the target object.

[0185] S228. Obtain a three-dimensional model based on the 2D hand joints and 3D hand vertex positions.

[0186] As Figure 8 shows an example of real-time constructing a three-dimensional model of a hand from a camera image, where the three-dimensional model can be, for example, a polygon mesh model.

[0187] Step 710. Input an image. For example, an RGB image captured by a camera can be passed to an open-vocabulary object detection model, such as YOLO-World or GroundingDINO. The open-vocabulary object detection model YOLO-World is described in detail in the literature "Cheng, Tianheng, Lin Song, Yixiao Ge, Wenyu Liu, Xinggang Wang, and Ying Shan. 'Yolo-world: Real-time open-vocabulary object detection.' In Proceedings of the IEEE / CVF Conference on Computer Vision and Pattern Recognition, pp. 16901 - 16911. 2024", and all of its disclosed content is incorporated into this disclosure. The open-vocabulary object detection model GroundingDINO is described in detail in the literature "Ren, Tianhe, Qing Jiang, Shilong Liu, Zhaoyang Zeng, Wenlong Liu, Han Gao, Hongjie Huang et al. 'Grounding DINO 1.5: Advance the "Edge" of Open-Set Object Detection.' arXiv preprint arXiv:2405.10300 (2024)", and all of its disclosed content is incorporated into this disclosure.

[0188] Step 720. Object detection. For example, the possible position and size of the hand, i.e., the bounding box of the hand, can be detected through the open-vocabulary object detection model. Optionally, the open-vocabulary object detection model can use the keyword "hand" as input to locate and detect the hand in the input image. These open-vocabulary object detection models can detect various object categories including the hand without retraining.

[0189] Step 730, Joint and vertex estimation. For example, an image with bounding box annotations can be processed by a simpleHand model (composed of a marker generator and a mesh regressor) to estimate 2D hand joint and 3D hand vertex positions respectively. The simpleHand model is described in detail in the literature "Zhishan Zhou, Shihao Zhou, Zhi Lv, Minqiang Zou, Yao Tang, and Jiajun Liang. A simple baseline for efficient hand mesh reconstruction. In Proceedings of the IEEE / CVF Conference on Computer Vision and Pattern Recognition (CVPR), pages 1367–1376, June 2024", and the entire content disclosed therein is incorporated into this disclosure. Further, the local camera coordinates of the hand vertices can be converted to 3D world coordinates together with the depth information.

[0190] Step 740, Mesh reconstruction. For example, a mesh of the hand can be created based on the hand vertices obtained in step 730 and the triangular mesh generated by a hand model (such as the MANO model). The MANO model is described in detail in the literature "Javier Romero, Dimitrios Tzionas, and Michael J. Black. Embodied hands: modeling and capturing hands and bodies together. ACM Transactions on Graphics, page 1–17, 2017", and the entire content disclosed therein is incorporated into this disclosure.

[0191] The human hand is a highly flexible and deformable structure with multiple joints and degrees of freedom. Different from rigid objects, its geometry continuously changes due to the bending, stretching, and rotational movements of the fingers and palm. Therefore, it is impractical to hard-code the fixed positions of the spheres used to represent the surface contour of the hand in a static frame because it cannot accurately capture the pose-related deformations. To solve this problem, the aforementioned adaptive surface sphere representation can be utilized to dynamically determine the vertex indices on the simpleHand mesh template where the spheres used to represent the surface contour of the hand are placed. This method ensures that the spheres used to represent the surface contour of the hand adapt to local geometric changes and deformations in real time. Figure 9 Shows a mesh model 810 of the hand and spheres 820 used to represent the surface contour of the hand superimposed in a possible hand pose. AlthoughFigure 9 An example is shown where the sphere 820 is superimposed on the mesh model 810 of the target object (such as a hand) as an output display. However, those skilled in the art can understand that in some embodiments, the sphere 820 can also be displayed alone as an output without displaying the mesh model 810 of the hand. This processing method can have many advantages. For example, after modeling a complex object with a sphere, when re-rendering the object, the computational amount can be reduced and the efficiency can be improved.

[0192] The method provided in the present disclosure for representing the surface contour of a target object is fundamentally different from the control point method used in the original RMPflow (Riemannian Motion Policy flow) algorithm. To cope with the limited computing power of the CPU-based architecture, see Figure 10 , the original RMPflow algorithm sets thick collision bubbles at the centers of the control points along the robotic arm components, thereby forming corresponding thick repulsive zones through the thick collision bubbles. By forming a virtual region with a non-zero spatial thickness (i.e., the thick repulsive zone), when the robot enters this region, it will be subjected to an increasingly strong repulsive force, thereby avoiding collisions or maintaining a safe distance. This representation of the robot is very rough and inaccurate, but only requires a relatively small number of spheres (correspondingly, the computational requirements for CPU-based collision calculations are much lower). However, this will generate a thicker and uneven repulsive zone near the object surface, thereby reducing the accuracy of the motion planning / control algorithm. Also, the original RMPflow algorithm also uses large collision sphere bubbles placed in the environment during training (usually less than 5 static collision spheres are placed in the training simulator), which is also to conform to the low computing power of the CPU, and usually very rough meshes are used to replace the robotic components so that the CPU can run its motion planning algorithm on the robot at a reasonable iteration rate (frames per second). In contrast, the algorithm of the present disclosure uses a GPU-based architecture for computing and simulation, and uses the "jit_compile" (Just-in-time Compile) acceleration technology in TensorFlow (an end-to-end open-source machine learning platform). Therefore, the present disclosure achieves faster operation than the original algorithm through a finer and more dynamic collision sphere representation, thereby ensuring higher control accuracy.

[0193] For the collision spheres generated by the method provided in the present disclosure for representing the surface contour of a target object, the specific application method is as follows:

[0194] The relevant parameters of the collision balls generated by this disclosure (such as the center position, radius, normal) and their related descriptions (including vertex indices and the names of the objects to which they belong) are saved in a text file. When the robot control algorithm provided by this disclosure requires information about the objects involved in the scene, it will execute the following statements to load from the corresponding text file:

[0195] Collision balls = torch.Tensor(np.loadtxt(colsphr_path, dtype=np.float64, delimiter=",")),

[0196] where 'colshpr_path' provides the path to the collision ball file saved for the object, which is formed using the object name. Once the collision balls are loaded into the scene, there is no need to distinguish which object they are generated by. Instead, only need to know which category they belong to: robot, target or obstacle, which can be assigned by the algorithm during loading. In this way, the proposed control method will be trained for general geometric primitives regardless of the complex shapes of the original 3D objects in different scenes. In addition, libraries such as PyTorch and TensorFlow can be used to implement batch calculations of metrics such as the minimum distance between spheres at low cost.

[0197] Based on the above method for representing the surface contour of the target object, this disclosure provides a lightweight, dual-functional GPU-accelerated digital twin reconstruction platform that operates in two complementary modes: simulation mode and real-time reconstruction mode.

[0198] In simulation mode, the GPU-accelerated collision ball physics engine calculates the poses of the objects and the robot by simulating the physical interactions based on the collision balls. The engine replicates the placement and movement of the 3D robot components and environmental objects, enabling rapid testing and iterative development of control algorithms in a virtual environment.

[0199] In real-time reconstruction mode, the platform processes external inputs from vision sensors (such as RGB and depth cameras) as well as the actual robot joint positions and other sensor data. By continuously integrating these real-world inputs, the digital twin can accurately reflect the changing state of the environment. This method ensures the consistency of the sphere-based collision representation regardless of whether the data is from simulated data or real-time sensor data, thus significantly reducing the simulation-to-reality transfer gap.

[0200] At the core of this platform is a unified collision ball calculation pipeline that fuses pre-generated sphere templates with real-time 3D poses (from simulation or real-time input) to calculate the precise positions of the collision balls. In this process, each collision ball is initially defined by its center c in the local coordinate system of the object r to define its rest pose, and the estimated pose of the object is used to perform the transformation to the world coordinate c w . In its most basic form, this pose is represented by a rotation matrix and a translation vector .

[0201] For a single rigid body transformation, the homogeneous transformation matrix T can be defined as:

[0202]

[0203] Where:

[0204] · R is a 3×3 rotation matrix,

[0205] · t is a 3×1 translation vector,

[0206] · 0 T is a 1×3 zero vector.

[0207] Using this matrix, the center c of the sphere in the world coordinate system can be calculated by the following formula w :

[0208]

[0209] Where the 1 in the above formula represents the scale factor of the homogeneous coordinates, which is used to unify the matrix operations of rotation and translation, so that the transformation can be represented as a single matrix multiplication.

[0210] For the scenario where the collision ball is connected to the link of a mobile robot or is part of a more complex kinematic chain, a chained homogeneous transformation is applied. In this case, the overall transformation formula of the chained homogeneous transformation is as follows:

[0211]

[0212] Where T world represents the homogeneous transformation matrix from the local coordinate system of the object of the collision ball to the world coordinate system, represents the homogeneous transformation matrix from the base coordinate system of the robot to the world coordinate system, represents the homogeneous transformation matrix from the link coordinate system of the robot to the base coordinate system, represents the homogeneous transformation matrix from the local coordinate system of the object of the collision ball to the link coordinate system of the robot.

[0213] The definition of each transformation matrix T is as described above. Then, calculate the position of the sphere in the world coordinate system:

[0214]

[0215] where c s is the center of the sphere in the object's local coordinate system of the collision sphere.

[0216] In some embodiments, all key computational processes, such as physical simulation, collision sphere calculation, and forward kinematics, are directly executed on the GPU. This architecture reduces the data transfer overhead between the CPU and the GPU, achieving a high frame rate of up to 80 frames per second, which in turn ensures efficient model training and robot control as well as minimal latency.

[0217] Figure 12 FIG. shows a schematic diagram of the high-level architecture of a dual-functional GPU-accelerated digital twin reconstruction platform in a specific embodiment of the present disclosure. The platform operates in only one mode at a time. In the simulation mode, the GPU-accelerated physical engine generates object poses, generating dynamic data for 3D robot components and environmental objects. In the real-time reconstruction mode, real-time sensor data (including image data collected by RGB and depth cameras, robot joint positions, and other sensor data) updates the digital twin. Regardless of which mode is adopted, all input data flows through the collision sphere calculation pipeline, which integrates it with pre-generated templates to generate representations of the collision spheres. These representations are then processed by a control algorithm, which outputs robot control instructions (e.g., joint angles) for the simulated robot or physical hardware. This structured pipeline ensures seamless switching of control between the simulation and the real world, ensuring the accuracy and consistency of the environmental representation.

[0218] In summary, the dual-functional GPU-accelerated digital twin platform of the present disclosure provides a powerful, efficient, and scalable solution for robotic applications in both the simulation and the real world. By integrating a GPU-accelerated collision sphere physical engine and real-time sensor inputs, the platform facilitates the seamless deployment of advanced motion planning and control algorithms, thereby not only accelerating the development cycle but also ensuring high-speed, low-latency execution across various operating environments.

[0219] In an alternative embodiment, the GPU-accelerated collision sphere physical engine represents each object as a collection of spheres, each sphere defined by a center position and a radius. Objects can be classified as dynamic objects or static objects. Dynamic objects (such as robots and other moving entities) respond to physical forces, while static objects (such as the floor and immovable obstacles) remain fixed. Then, the engine effectively detects collisions through the following evaluation conditions:

[0220] |c i-c j |<r i +r j

[0221] where c i and c j are the centers of two spheres, and r i and r j are their respective radii. When this inequality holds, a collision is detected, and the penetration depth is calculated as follows:

[0222] δ ij =(r i +r j ) - |c i -c j |

[0223] In addition, the collision normal is determined by the following formula:

[0224]

[0225] where ∈2 is a small constant that serves to prevent the denominator of the above formula from being zero.

[0226] Once a collision is detected, the engine calculates the displacement vector based on the penetration depth and the collision normal to resolve the collision. If both objects are dynamic, each object translates half of the required displacement along the normal, so the new positions of the two objects are determined by the following formula:

[0227]

[0228] where p i,old and p j,old represent the previous positions of the two objects, p i,new and p j,new represent the new positions of the two objects, and α is a response factor that can be adjusted to control the sensitivity of the collision response. If one of the objects is static, only the dynamic object moves the full displacement, and the new position of the dynamic object is determined by the following formula:

[0229] p dynamic,new =p dynamic,old ±αδ ij n

[0230] where p dynamic,old represents the previous position of the dynamic object, and p dynamic,new represents the new position of the dynamic object.

[0231] The above processing method can ensure the effective solution of the mutual penetration problem and reposition the objects to prevent further collisions in subsequent simulation steps. When multiple spheres within a single object collide simultaneously, each collision is iteratively resolved. By leveraging vectorized operations on the GPU, the engine can perform these calculations in parallel for a large number of spheres, providing fast and scalable collision detection and solutions. This processing method is seamlessly integrated with the unified collision sphere calculation pipeline of the present disclosure, enhancing the overall responsiveness and physical accuracy of the digital twin reconstruction platform.

[0232] Riemannian Motion Strategy

[0233] In an alternative embodiment, the present disclosure obtains the desired acceleration of each joint of the robot through a Riemannian motion policy. The Riemannian motion policy (RMP) is a geometric framework for robot motion control that decomposes complex motion tasks into multiple simple subtasks (such as obstacle avoidance, goal attraction, joint limits, etc.) and combines the motion policies of these subtasks within the framework of Riemannian geometry. The core idea is to define a Riemannian metric for each subtask, which encodes the importance or directionality of the corresponding task, and then geometrically fuse the acceleration policies of these subtasks into a globally consistent policy. The algorithm of the Riemannian motion policy in the present disclosure may specifically be the RMPFlow algorithm (for the RMPFlow algorithm, see the literature "C.A. Cheng, M. Mukadam, J. Issac, S. Birchfield, D. Fox, B. Boots, and N. Ratliff. Rmpflow: A computational graph for automatic motion policy generation. Algorithmic Foundations of Robotics XIII: Proceedings of the 13th Workshop on the Algorithmic Foundations of Robotics, Springer International Publishing, pp. 441 - 457, 2020", the entire content disclosed in this literature is incorporated into the present disclosure) or the RMP2 algorithm (for the RMP2 algorithm, see the literature "A. Li, C. Cheng, M.A. Rana, M. Xie, K.V. Wyk, N. Ratliff, and B. Boots. RMP2: A structured composable policy class for robot learning. arXiv preprint arXiv:2103.05922, 2021", the entire content disclosed in this literature is incorporated into the present disclosure), and preferably the RMP2 algorithm is adopted.

[0234] In an alternative embodiment, after obtaining the first sphere representation information and the second sphere representation information in step S1, in step S2, the selection and definition of control points are performed. The control points serve as geometric anchors for subtasks, and the calculation of the task space is based on these control points. Among them, for the robot, the center of each sphere in the first sphere representation information can be used as a control point, and for the target object, the center of each sphere in the second sphere representation information can be used as a control point.

[0235] In an alternative embodiment, the present disclosure presets a number of subtasks, and defines the task space and sets the RMP parameters for each subtask. The specific subtasks may include target attraction, collision avoidance, joint limit, joint velocity cap, end effector orientation alignment, end effector normal direction alignment, etc. For each subtask, task space variables, Riemannian metrics, and acceleration strategies are defined in its corresponding task space. For the first 4 subtasks (target attraction, collision avoidance, joint limit, joint velocity cap), the definitions of the task space variables, Riemannian metrics, and acceleration strategies in their corresponding task spaces refer to the RMP2 paper (see the literature "A. Li, C. Cheng, M. A. Rana, M. Xie, K. V. Wyk, N. Ratliff, and B. Boots. RMP2: A structured composable policy class for robot learning. arXiv preprint arXiv:2103.05922, 2021", and the entire content disclosed in this literature is incorporated into the present disclosure) and the code provided by it (https: / / github.com / UWRobotLearning / rmp2); the definitions of the task space variables, Riemannian metrics, and acceleration strategies in the task spaces of the end effector orientation alignment subtask and the end effector normal direction alignment subtask are also set as defined in the task space of the target attraction subtask, but the current end effector position and the target position therein are replaced by the current end effector attitude vector and the target attitude vector or the current end effector normal vector and the target normal vector.

[0236] In an alternative embodiment, the definitions and code representations of the task space variables, Riemannian metrics, and acceleration strategies in the task space corresponding to the end effector orientationalignment subtask are as follows:

[0237] Task space variables:

[0238] Input: x,x d ,x g , parameters include: a o_gain ,a d_gain , m s , n batch , m max ,m min , b prox ; Riemannian metric:

[0239] m o = b m *m o ;

[0240] where b m = b a_orien t*b prox +(1 - b a_orient );

[0241] b a_orient = exp(-0.5*d s_boost );

[0242] d s_boost = δ norm / b prox ;

[0243] m o = w o *m max I+(1 - w o )*m min *S o ;

[0244]

[0245] I = eye(n batch );

[0246] w o = (1 - m min )*exp(-0.5*d s )+m min ;

[0247] d s = δ norm / m s ;

[0248] Acceleration strategy:

[0249] a o =a o_gain *δ o / (δ norm +∈ norm )-a d_gain *x d ;

[0250] in,

[0251] δ soft =max{δ norm ,∈ norm / 10*tf.ones_like(δ norm )};

[0252] δ norm (i)=||δ o (i)||;

[0253] δ o =x g -x;

[0254] Among them, x g represents the target posture, x d represents the pose velocity (the rate of change of pose over time), and x represents the pose of the end effector.

[0255] a o_gain Indicates the acceleration coefficient in the attitude direction, a d_gain Indicates the damping coefficient in the attitude direction, m s The metric scale for the attitude direction, n batch Indicates the batch processing size of the end effector collision sphere, m max The maximum metric representing the attitude direction, m min The minimum metric representing the attitude direction, b prox represents the length basis of the proximity metric enhancement. These parameters can be tuned in practice.

[0256] In an optional embodiment, the definitions and code representations of the task space variables, Riemannian metrics, and acceleration strategies in the task space corresponding to the end effector normal direction alignment subtask are as follows:

[0257] Task space variables:

[0258] Input: x, x d , x g , parameters include: an_gain , a d_gain , m s ,n batch , m max ,m min , b prox ;

[0259] Riemannian metric:

[0260] m n =b m *m n ;

[0261] Among them, b m =b a_norm *b prox +(1-b a_norm );

[0262] b a_norm =exp(-0.5*d s_boost );

[0263] d s_boost =δ norm / b prox ;

[0264] m n =w n *m max I+(1-w n )*m min *S n ;

[0265]

[0266] I=eye(n batch );

[0267] w n =(1-m min )*exp(-0.5*d s )+m min ;

[0268] d s =δ norm / m s ;

[0269] Acceleration strategy:

[0270] a n =a n_gain *δ n / (δ norm +∈ norm )-a d_gain *x d ;

[0271] Among them,

[0272] δ soft = max{δ norm , ∈ norm / 10 * tf.ones_like(δ norm )};

[0273] δ norm (i) = ||δ n (i)||;

[0274] δ n = x g - x;

[0275] Among them, x g represents the target normal vector, x d represents the normal vector velocity (the rate of change of the normal vector over time), and x represents the normal vector of the end effector.

[0276] a n_gain represents the acceleration coefficient in the direction of the normal vector, a d_gain represents the damping coefficient in the direction of the normal vector, m s represents the metric scale in the direction of the normal vector, n batch represents the batch processing scale of the end effector collision sphere, m max represents the maximum metric benchmark in the direction of the normal vector, m min represents the minimum metric benchmark in the direction of the normal vector, b prox represents the length benchmark for enhancing the proximity metric, and these parameters can be adjusted in practice.

[0277] In an alternative embodiment, step S2 maps the task space to the configuration space and fuses the global policy through the Riemannian motion strategy. Specifically, for each control point, the Jacobian matrix from the task space to the configuration space (such as the joint space) is calculated, and then the Riemannian metric and acceleration strategy corresponding to the task space are transformed to the configuration space through the Jacobian matrix to ensure geometric consistency. For the fusion of the global policy, specifically, a weight matrix corresponding to the Riemannian metric is preset for each subtask and the weights are dynamically adjusted, and finally all the weight matrices are superimposed to form the final fusion matrix, that is, all subtasks are weighted and fused through the Riemannian metric, and finally the global acceleration, that is, the desired acceleration, is obtained by solving.

[0278] In the specific implementation process, the input of the Riemannian motion strategy mainly includes the definition of the task space (Task Spaces) corresponding to each subtask, the RMP parameters of each subtask (including the Riemannian metric and acceleration strategy), and the differentiable forward kinematics function of the robot.

[0279] The differentiable forward kinematics function is used to describe the mapping relationship of the robot from the joint space to the Cartesian space (or task space). That is, the Jacobian matrix from the task space to the configuration space (such as the joint space) for each control point can be specifically calculated and implemented through the differentiable forward kinematics function.

[0280] The differentiable forward kinematics function can calculate the position and orientation of the robot's end effector or other interested parts in the three-dimensional space according to parameters such as the angles and lengths of the robot's various joints. Through the differentiable forward kinematics function, the current state of the robot's task space can be calculated. By comparing the current state with the preset desired state, the state error can be obtained. In the Riemannian motion strategy, the corresponding acceleration can be calculated according to the state error and the Riemannian metric.

[0281] Using the differentiable forward kinematics function, the state information of the robot in the task space can be continuously provided for the Riemannian motion strategy, enabling the strategy to calculate the appropriate acceleration according to the current state and task objectives, and mapping it to the joint space through the Jacobian matrix to achieve precise control of the robot. At the same time, during the movement, the differentiable forward kinematics function can also be used to monitor in real time whether the robot will collide with obstacles, so as to adjust the motion strategy in a timely manner.

[0282] Optionally, the input may further include the current state information of the robot (i.e., the real-time operating parameters of the robot, preferably including real-time joint angles or poses, and real-time joint velocities or end effector velocities). Optionally, the input may further include environmental information, such as obstacle positions, target positions, etc.; optionally, the input may further include the dynamic model of the robot, such as the inertia matrix, Coriolis force, gravity, etc., so that the Riemannian motion strategy can generate physically more feasible control commands by combining this information. The output of the Riemannian motion strategy includes the desired acceleration.

[0283] In an alternative embodiment, the target object can be two-dimensional or three-dimensional, and the number can be one or more.

[0284] In an alternative embodiment, the information to be processed further includes the third sphere representation information corresponding to the floor or wall. Considering the third sphere representation information during the relevant operations of the Riemannian motion strategy can solve the error problem caused by the non-strictly horizontal attitude of the tabletop when the robot is placed on the table.

[0285] In an alternative embodiment, the information to be processed further includes the target position information of the end effector. Among them, the target position information of the end effector can be one of the key considerations for sub-task decomposition in the Riemannian motion strategy, and it can participate in the construction of the geometric dynamic system of the corresponding sub-task. In each control cycle, first use the forward operation to recursively propagate the position and velocity information in each sub-task space, and obtain the acceleration of the sub-task space according to the geometric dynamic system. Here, the position information includes the current position and the target position of the end effector in the world coordinate system (which can be represented by the target world coordinates). By comparing the two, the position error can be calculated, and then the acceleration can be determined according to the geometric dynamic system to guide the end effector to move towards the target world coordinates. Then use the pull-back operation to fuse the accelerations in each sub-task space in the form weighted by the Jacobian matrix and its dual matrix of the sub-task space. ， Finally, the acceleration of the global task is obtained to achieve the control of the overall movement of the robot, so that the end effector can move towards the target world coordinates along the desired trajectory.

[0286] In an alternative embodiment, the information to be processed further includes the translation parameters of the base. During the control process of the robot, when it involves moving in a large-scale space, or when the target position to be moved to is far from the initial position, or when it is necessary to optimize the path and time, the translation parameters of the base can be further considered. At this time, the translation parameters of the base can be used as one of the considerations for sub-task decomposition and the establishment of the geometric dynamic system, so as to be able to expand the moving space, generate more optimized motion trajectories and control strategies, and improve the adaptability of the robot to different tasks and working environments.

[0287] In an alternative embodiment, in step S4, the desired acceleration is combined with the operating frequency of the robot to generate the angles of each joint at the next moment. In step S5, these joint angles are sent to the robot as control commands, and the robot will call the built-in engine to reach the specified position. Optionally, if the robot uses speed or acceleration control, the desired acceleration can be directly sent to the robot as a control command, or the desired acceleration can be integrated to obtain the speed and the corresponding speed control command can be generated and sent to the robot. Optionally, if the robot uses torque control, the desired acceleration can be converted into the actual joint torque and the corresponding torque control command can be generated and sent to the robot.

[0288] In an alternative embodiment, the present disclosure utilizes a Riemannian motion strategy to calculate a corresponding acceleration value for each subtask. Specifically, for the target attraction subtask, its input parameters include the second sphere representation information corresponding to the target position on the target object to be approached and use it as the target movement position of the end effector. The input parameters also include the differentiable forward kinematic function of the robot and the current joint angles and speeds of the robot. Then, the normalized distance or squared distance between the target position and the current position of the end effector is calculated, and based on the normalized distance or squared distance, the Riemannian metric as a weight and the corresponding acceleration value can be calculated.

[0289] In an alternative embodiment, for the collision avoidance subtask, its input parameters include a second sphere set representing one or more target objects in the target scene and a first sphere set representing one or more robot components. The input parameters also include the differentiable forward kinematic function of the robot and the current joint angles and speeds of the robot. Then, the pairwise distances between the first spheres and the second spheres can be calculated based on the first sphere set and the second sphere set (specifically, by traversing the first spheres in the first sphere set and the second spheres in the second sphere set and calculating the pairwise distances), and the sum of the soft collision cost functions for all sphere pairs is calculated (the corresponding function value is always zero for each pairwise distance exceeding the threshold distance), and based on the sum of the soft collision cost functions, the Riemannian metric as a weight and the corresponding acceleration value are calculated.

[0290] In an alternative embodiment, for the end effector orientation alignment subtask, its input parameters include the target world coordinates of the end effector, the differentiable forward kinematic function of the robot, and the current joint angles and speeds of the robot. Then, based on the dot product function, the difference between the current normal and tangent vectors of the robot end effector in the world coordinate system and the desired normal and tangent vectors of the robot end effector can be calculated, and based on the difference, the Riemannian metric as a weight and the corresponding acceleration value are calculated.

[0291] In an alternative embodiment, for the collision avoidance subtask, considering the case of large obstacle collision avoidance, the input parameters include a second set of spheres representing one or more target objects in the target scene and a first set of spheres representing one or more robot components. The input parameters further include the differentiable forward kinematic function of the robot and the current joint angles and speeds of the robot. Then, according to the collision avoidance subtask, the corresponding Riemannian metric as a weight and the corresponding acceleration value are calculated. Then, based on the distances from the robot to the obstacles and the floor, the Riemannian metric as a weight and the corresponding acceleration value finally corresponding to the large obstacle collision avoidance subtask are calculated.

[0292] Large Obstacle Avoidance

[0293] Large obstacle collision avoidance needs to consider the situation where large obstacles block the shorter path for the robot to reach the target position, so a large degree of detour is required to find another feasible route to reach the target position. For traditional methods to solve this problem, such as the Potential Field Method (PFM) and the Model Predictive Control (MPC), they often fall into the local optimal situation, that is, they will fall into some local optimal points where the attraction of the target and the repulsion of the large obstacles are balanced, resulting in the robot stagnating or oscillating, and thus unable to guide the robot to reach the target position without colliding with large obstacles. To solve the problems existing in the traditional methods, in an alternative embodiment, the present disclosure uses a Neural Network (NN) to generate the additional acceleration a of each joint of the robot residual , and superimpose it on the desired acceleration a obtained through the Riemannian motion strategy in step S2 base to obtain the joint acceleration a joint . The calculation formula of the joint acceleration is as follows:

[0294] a joint = a base + w * a residual ;

[0295] where, in an alternative embodiment, the additional acceleration a residual and the desired acceleration a baseIt can be the overall vector composed of all joint accelerations, where w is a positive weight, and preferably w = 0.1. Of course, if more refined regulation is required, the acceleration of each joint can be calculated separately, and different weights w can be set for each joint according to its actual situation.

[0296] For example, Figure 11 FIG. shows a schematic diagram of the architecture of a neural network NN provided according to some embodiments of the present disclosure. The neural network NN can adopt a Residual Neural Network (ResNN). More preferably, the neural network NN can also adopt a Residual Neural Network Reinforcement Learning (ResNNRL) that combines a residual neural network and a reinforcement learning algorithm. Thus, the present disclosure uses an RL training framework to generalize the entire complex scenario of robot control into a general sphere-based representation, combines the classical robot control algorithm of Riemannian motion strategy with a deep reinforcement learning control algorithm. This algorithm further refines the classical robot actions defined by the Riemannian motion strategy and considers physical symmetry during the formation of its neural network to minimize the training data requirements. In the representation of the present disclosure, the robotic arm and multiple objects (with different types / categories) look like a set of surface spheres for both the Riemannian motion strategy and the RL neural network. This enables the system to be adjusted / trained in a small number of example environments while ensuring that the trained system can still work in a wider range of environments outside the distribution of its training environment.

[0297] The input of the neural network NN includes the distance vector from the robot to the obstacle, the three-dimensional pose difference vector between the current position and pose of the end effector and the target position and pose, and a customized reward that encourages positive behaviors (such as getting closer to the target, retracting towards the robot base to avoid large obstacles, etc.).

[0298] In an alternative embodiment, the neural network NN is designed to be able to identify difficult situations such as when the robot gets stuck in a local optimum, and with the help of user-defined rewards, generate appropriate adjustment actions to cope with the challenges encountered. To address the problem of obstacle avoidance for large obstacles, in some embodiments of the present disclosure, the residual neural network - reinforcement learning neural network ResNNRL can adopt neural network models such as MLP (Multilayer Perceptron), LSTM (Long Short-Term Memory), and Transformer. For example, MLP, LSTM, and Transformer neural network models can be designed. After configuring the structures of the MLP / LSTM / Transformer neural network models to have less than 10 layers (with the weights of each layer not exceeding 1000), they can all understand the situation where the robot gets stuck in a local optimum and successfully generate the additional acceleration to guide the robot out of the local optimum and reach the target position.

[0299] In an alternative embodiment, for the large obstacle avoidance task, the corresponding defined rewards include an inward reward and an upward reward. The reason for rewarding inward movement is that when the surrounding space is occupied by many obstacles, the robot moving inward to the fixed base and shrinking itself can reduce the probability of colliding with external obstacles and leave some space for rotating its downstream linkages. In addition, since most of the objects in the scenarios considered here are located on the tabletop, the robot moving upward can sometimes help the robot find a feasible trajectory to bypass large obstacles and reach the target position. However, it should be noted that when the robot approaches the innermost or uppermost position, these behaviors should not be given more rewards. Specifically, the codes corresponding to the inward reward and the upward reward are as follows:

[0300]

[0301]

[0302] Among them, the constant "catch_thre" in the above code represents a threshold such that the total reward_escape reaches the maximum value. And when the distance between the end effector and the target is less than this threshold, the inward and upward rewards are replaced by a reward defined by the distance from the end effector to the target position, and this defined reward can be set according to actual needs. Experiments show that these rewards can help the robotic arm bypass large obstacles and find a feasible route to reach the target.

[0303] In an alternative embodiment, the neural network NN may further include a Permutation-Invariant Neural Network (PINN). The neural network NN processes the output from the above-mentioned ResNN residual neural network or ResNNRL residual neural network - reinforcement learning neural network through the PINN network, which can further refine the RMP metric and acceleration through the PINN permutation-invariant neural network. The core feature of the PINN network is its insensitivity to the order of the input data, ensuring that the PINN network produces the same output regardless of the order in which the obstacles (each obstacle is represented by a collision ball) appear. This invariance is achieved by aggregating features using order-independent operations.

[0304] Through the PINN network, the neural network NN is designed to process both fixed-length and variable-length inputs, thus maintaining robustness and invariance with respect to the sorting of obstacle features. For example, in one example of the neural network NN, the neural network NN may include a Transformer network and a PINN network. The fixed-length inputs (including robot joint configuration data) are processed through a series of fully connected layers of the Transformer network to extract basic features. The variable-length inputs are divided by the Transformer network into three parts: an activation mask for identifying valid obstacles and two sets of obstacle-related features. Specifically, one variable represents the vector distance from the end effector of the robot to the collision ball, denoted as v offset , while the other variable captures the attributes of the collision ball (including three-dimensional coordinates and radius), denoted as v sphere .

[0305] For these variable-length parts, the Transformer network applies the multi-head attention mechanism to the collision offset and collision ball features respectively, effectively capturing the interactions and dependencies between obstacles. According to the attention mechanism, each set of outputs is refined through a two-level feed-forward network. To ensure that only active obstacles contribute to the final representation, the output is multiplied element-wise by the extended activation mask, and then the average value of the valid elements is taken. Mathematically, if o i represents the output of the i-th obstacle, and m i is its corresponding mask value, then the aggregated feature is calculated as follows:

[0306]

[0307] This process produces the average features of the collision offset and collision ball features, denoted by and It is represented. Then, the PINN network can connect these average vectors with the fixed input feature f to form an aggregated feature vector:

[0308]

[0309] The aggregated feature vector x then passes through a series of fully connected layers of the PINN network to produce the final output. Denote the activation function as φ, and the weights and biases of the j-th dense layer as W j and b j , then the output y is given by the following formula:

[0310] y = φ L (W L φ L-1 (…φ1(W1x + b1)…)+b L )

[0311] The above formula can ensure that the final output of the neural network NN remains unchanged for any permutation of the input colliding balls and objects, thereby enhancing its robustness in dynamic real-world scenarios.

[0312] In an alternative embodiment, the training process of the neural network of the present disclosure is as follows:

[0313] S11. Obtain input parameters;

[0314] Among them, the input parameters include a second sphere set representing one or more target objects in a stationary pose in the target scene and a first sphere set representing one or more robot components in a stationary pose; the input parameters also include the differentiable forward kinematic function of the robot, the current joint angles and speeds of the robot, the initial 6D poses of all target objects and the robot base, and the target world coordinates of the robot end effector;

[0315] S12. Calculate the positions of the first spheres corresponding to the robot and the second spheres corresponding to the target objects at the current moment;

[0316] S13. Iteratively generate control instructions for the robot by using the control method of the robot of the present disclosure;

[0317] S14. Use the generated control instructions and the current joint angles and speeds of the robot to iteratively update the next set of joint angles and speeds of the robot at the next moment;

[0318] S15. Iteratively update the relevant parameters of the neural network according to the simulation trajectory results of the robot until the training of the neural network is completed and the trained neural network model is exported.

[0319] In an alternative embodiment, step S11 further includes: transferring all initial input parameters from the CPU to the GPU, and the calculations in step 12 are specifically implemented in the GPU.

[0320] In an alternative embodiment, the present disclosure can implement fast training and inference of GPU-based RMP2-RL (Riemannian Motion Policy 2-Reinforcement Learning). The GPU-based simulator of the present disclosure is a unified collision sphere calculation pipeline, which can transform pre-generated sphere templates with real-time three-dimensional postures to obtain accurate collision sphere positions. All core calculation processes, including physical simulation, collision sphere calculation, and forward kinematics, are directly executed on the GPU. In addition, the GPU-based simulator provides methods that can return the current states (such as positions, orientations, speeds, etc.) of each robot joint. These methods are also implemented on the GPU, thereby further accelerating the simulation process.

[0321] In addition, the simulator of the present disclosure is equipped with a GPU-accelerated collision sphere physical engine, which calculates the postures of objects and robots by simulating physical interactions based on collision spheres. The engine replicates the placement and movement of three-dimensional robot components and environmental objects, thereby ensuring effective resolution of interpenetration and repositioning of objects in subsequent steps. By using vectorized operations on the GPU, the physical engine can perform these calculations in parallel for a large number of spheres, ensuring fast and scalable collision detection and resolution. Therefore, compared with other CPU-based simulators such as Pybullet, this architecture minimizes the CPU-GPU data transfer overhead and achieves more efficient model training and low-latency robot control.

[0322] The present disclosure also implements GPU-based calculations. Using the information required by the GPU-based simulator, the "get_reward()" method can be used to calculate defined rewards to motivate the robot to take favorable actions (for example, moving inward / contracting itself to avoid large-scale obstacles, moving upward to bypass most obstacles on the table). In addition, the platform of the present disclosure also provides methods for creating and clearing targets and objects in the simulator, just like at the start, reset, or end of an episode in Pybullet. The simulation using the simulator of the present disclosure can run entirely on the GPU, thereby minimizing the need for CPU-GPU data transfer. Specifically, to obtain object metrics, proceed to the next step, or update the state in the simulation scenario, the present disclosure uses the following functions provided in the simulator:

[0323] 1) stepSimulation(), which is used to update the environment after applying the operations in the current step;

[0324] 2) getLinkState(robot.robot_uid, robot.eef_uid), which is used to obtain the state of the specified robot link in the current scene (for example, the state of the end effector of the robot with ID robot_uid is obtained here);

[0325] 3) getClosestPoints(robot.robot_uid, obs_uid, distance_threshold), which is used to obtain the closest points between the robot with ID robot_uid and the obstacle with ID obs_uid within the distance threshold distance_threshold;

[0326] 4) getBasePositionAndOrientation(robot.robot_uid), which is used to obtain the reference position and orientation in the world coordinate system;

[0327] 5) Calculate the ViewMatrix of YawPitchRoll(

[0328] cameraTargetPosition = base_pos,

[0329] distance = cam_dist,

[0330] yaw = cam_yaw,

[0331] pitch = cam_pitch,

[0332] roll = 0,

[0333] upAxisIndex = 2,

[0334] ), which is used to obtain the viewing matrix of the camera, including the reference position of the camera, the distance from the origin of coordinates, (yaw, pitch, roll) angles, and the index of the axis pointing upward in the view;

[0335] 6) Calculate the projection matrix FOV(

[0336] fov = 60,

[0337] aspect = float(RENDER_WIDTH) / RENDER_HEIGHT,

[0338] nearVal = 0.1,

[0339] farVal = 100.0,

[0340] ) for calculating the field of view of the projection matrix for rendering;

[0341] 7) getCameraImage(

[0342] width = RENDER_WIDTH,

[0343] height = RENDER_HEIGHT,

[0344] viewMatrix = view_matrix,

[0345] projectionMatrix = proj_matrix,

[0346] renderer = ER_BULLET_HARDWARE_OPENGL,

[0347] ) for obtaining an image according to the specified view matrix and projection matrix;

[0348] 8) removeBody(obj) for deleting a specified object in the simulation environment;

[0349] 9) resetSimulation() for resetting the simulation by restoring the default initial environment;

[0350] Compared with the Pybullet simulator, the simulator of the present disclosure can track the state of the robot and includes methods for creating and clearing targets and objects. Therefore, replacing the Pybullet simulator with the renderer / simulator of the present disclosure will significantly improve the execution speed of the robot learning / control algorithm. Subsequently, the present disclosure can implement RL algorithms, such as proximal policy optimization (PPO), to train the network model during the movement of the robot. As described above, in order to train the model to achieve high-level goals, the RL algorithm requires metrics of the object state and appropriate rewards defined by the user in the environment as inputs to motivate optimization behaviors that are beneficial to achieving high-level goals. In the interaction between the environment and the robot, the model weights are iteratively improved through the reinforcement learning algorithm. After training in a sufficiently diverse environment, the RL algorithm will generate robust residual actions to help the robot cope with challenging scenarios by adjusting the original benchmark RMP actions.

[0351] In an alternative embodiment, the present disclosure also uses acceleration techniques in TensorFlow and PyTorch. Specifically, batch operations on the GPU are used to accelerate the for-loop operations on the CPU. Specifically, in the original RMP algorithm, the closest points between the obstacles and the robot spheres are calculated in a for-loop by applying torch.norm() and torch.argmin() to each robot sphere. This process is sequential and slow. In the present disclosure, they can be replaced with the batch operators torch.cdist() and torch.argmin(*, dim=1) that are parallelly computed on the GPU, thus significantly improving the processing speed.

[0352] In an alternative embodiment, to further exploit the computing power of TensorFlow on the GPU, the present disclosure decorates the RMPFlow method with @tf.function(jit_compile=True). This method requires a warm-up process during the initialization of the control class to ensure the GPU resources of the control module. This decoration enables the algorithm of the present disclosure to pre-compile the entire RMP TensorFlow computation graph during the initialization phase, and in subsequent RMP2 reinforcement learning, regardless of how the environment changes, the pre-compiled computation graph is used to perform the joint action computation. The new code makes the two differential processes (forward push and backward pull) and the forward kinematics of the RMPFlow algorithm about 10 times faster than the original version.

[0353] In an alternative embodiment, to further accelerate the algorithm of the present disclosure, a faster custom version can be used to replace the forward kinematics function from the pytorch-kinematics library in the original algorithm. This version also performs batch operations instead of for-loop or recursive operations. The advantage of this faster custom version compared to the recursively implemented forward kinematics is that it calculates the transformation of each joint only once, avoiding repeated calculations, thus improving the calculation speed. Different from the forward kinematics that calculates each joint one by one in a for-loop, this algorithm uses multiple batch operation functions of Pytorch / Tenstorflow to simultaneously calculate the transformations corresponding to all joints, thus further improving the calculation speed. Using these new technologies, the method of the present disclosure can achieve a rate of 45 - 80 FPS even when using a finer and more adaptive sphere representation.

[0354] Specifically, in the implementation of traditional GPU-compatible differentiable robot forward kinematics matrix calculation (such as the PyTorch-Kinematics library), the code is written in a recursive manner suitable for CPU operations rather than being adapted to the GPU architecture. For example, a typical forward kinematics library will first evaluate the parent link of the robot (usually called the "base link"), calculate its constant offset translation / rotation matrix, then calculate the additional translation / rotation matrix of the robot component based on the input joint angles, and finally generate the transformation matrix of the parent link. Subsequently, the above process is repeated for each child link of the parent link, and the final transformation matrix of the child link is multiplied by the parent link transformation matrix. This process is recursively executed on the child nodes at all levels of the child links until the robot forward kinematics tree no longer contains child nodes. This recursive algorithm may cause performance bottlenecks when calculating the robot parent link on the GPU.

[0355] In an alternative embodiment, to address this inefficiency, the present disclosure pre-builds constant rotation / translation matrices (constant matrices only need to be calculated once) for each link coordinate system during the software initialization phase. The present disclosure will pre-calculate the maximum tree branch length n_max (i.e., the longest recursive parent-child chain from the base link to the farthest child link) during software initialization. During each forward kinematics function call, the present disclosure creates a set of n_batch×n_link×n_max×4×4 identity transformation matrices - where n_batch corresponds to the number of groups of joint angles to be evaluated (usually 1 in most cases), and n_link corresponds to the number of robot links. At the initial stage of the iteration, these matrices are all 4×4 identity matrices. The present disclosure calculates the transformation matrix corresponding to each input joint angle and modifies the relevant indices in the n_batch×n_link×n_max×4×4 matrix set according to the calculated joint angle transformation matrix. Note that many elements in this large matrix will remain as 4×4 identity matrices.

[0356] Similar to the recursive version, the present disclosure multiplies these additional translation / rotation matrices by the pre-calculated static offset matrices. However, different from the recursive version that needs to perform composite transformation matrix multiplication serially along each branch across multiple loops, the parallel forward kinematics method adopted by the present disclosure only needs to perform composite transformation matrix multiplication through a single loop along the n_max dimension, and finally outputs the required set of n_batch×n_link×n_max×4×4 forward kinematics transformation matrices. Although this version will perform a large number of "ineffective" identity matrix multiplications (M = M×I), when running on the GPU, this method is usually more than 4 times faster than the traditional recursive forward kinematics version.

[0357] Of course, those skilled in the art should understand that the above algorithm including the customized version is optional and not the algorithm that must be adopted. The overall algorithm of the present disclosure can also adopt the forward kinematics function of other versions. Except that the calculation speed is slightly worse, adopting the algorithms of other versions does not affect the technical implementation of the present disclosure.

[0358] In an optional implementation manner, the basic implementation process of the present disclosure is as follows:

[0359] To train the RL model of the present disclosure in different environments, first, a meaningful scene with large obstacles is randomly generated, where the positions and postures of the objects are uniformly selected within a given range, while ensuring that they do not overlap with each other. The trained neural network parameters are loaded using the function onnxruntime.InferenceSession(model_path,...) and saved using the function defined in the RL algorithm class (here it is PPO) adopted in the method of the present disclosure, export_policy_model(path, onnx).

[0360] To test the trained model in a virtual environment, the output action (i.e., acceleration) of the algorithm of the present disclosure will first be converted into the increment of the robot joint angle according to the fixed time step preselected according to the overall FPS of the system. Then, the joint angle increment is passed to the customized GPU-based simulator of the present disclosure, where the objects including the robot, obstacles, and target have been loaded into the scene. Subsequently, the simulator will apply the next action of the robot specified by these robot joint angle increments and update the states of other objects at this moment. At the same time, it also returns indicators such as the position and posture of the end effector, the shortest distance vector between the sphere corresponding to the robot and the sphere corresponding to the obstacle, and calculates the customized reward of the present disclosure based on these updates. All information will be used as the input of the policy neural network, the RL algorithm return calculation, or the RMPFlow program to generate a new action in the next step. These processes are repeated in the virtual environment until the robot collides with an obstacle or reaches the target position and reaches the termination state.

[0361] Deploying the trained model in a real physical environment also has a similar workflow. First, use the output acceleration of the present disclosure's algorithm and a fixed time step set according to the overall FPS of the system to calculate the increments of the robot joint angles and joint angular velocities. Next, the new joint angles (equal to the current joint angles plus the joint angle increments) will be sent to the robot motors to drive the robot joints to the received angles. The two cameras in the robot system of the present disclosure will observe the changing scene after the robot moves. Then the captured pictures will be input into the vision module, such as the segmentation and pose estimation unit, to obtain the updated states of the robot and other objects, such as positions, poses, and whether a collision occurs. Using the state information, the robot control algorithm of the present disclosure will calculate metrics including a customized reward and the shortest distance vector between the sphere corresponding to the robot and the sphere corresponding to the obstacle. These metrics and state information will be input into the trained policy neural network, RL framework, and RMPFlow program to generate a joint acceleration for the next action. Then, the posture of the real robot will be adjusted to the new joint angles as in the previous round, and the workflow continues until a termination state is reached.

[0362] Compared with the prior art, the RL training platform of the present disclosure has a lighter and faster renderer / simulator based on GPU, which can significantly speed up the training and testing workflows of new motion planning / control algorithms. In addition, by moving all joint dynamics (including forward kinematics), simulating complex robot joint behaviors on GPU and eliminating the latency caused by CPU-GPU data transfer, the execution speed of the autonomous robot algorithm is further improved. Therefore, training planning / control algorithms such as RMP2-RL on the RL training platform of the present disclosure is at least 50% faster than platforms without these functions, thus achieving high responsiveness and low-latency control. So far, Gazebo, Coppeliasim, or other traditional robot training platforms do not have these capabilities. On the other hand, the method of the present disclosure can flexibly customize most modules on the RL training / testing platform. For example, algorithm developers can define their own robot and object classes and their kinematics running on GPU according to their applications. They can also combine the training environment with a lighter and faster renderer / simulator to optimize their control latency. Moreover, the platform is completely open, enabling them to implement the customized high-level planning / RL algorithms and low-level control algorithms they attempt to design and verify, thus making their development process faster and more efficient.

[0363] Embodiment 2

[0364] This embodiment provides a control system for a robot, the control system including: one or more processors, and the one or more processors are used to execute a computer program, such as to implement the method in Embodiment 1.

[0365] Example 3

[0366] Figure 13 As shown in the structural schematic diagram of an electronic device according to an exemplary embodiment of the present disclosure, the electronic device includes a memory, a processor, and a computer program stored in the memory and configured to run on the processor. When the processor executes the computer program, the method of any of the above embodiments is implemented. Figure 13 The electronic device 50 shown is merely an example. For example, those skilled in the art can understand that some processors, such as ASIC processors, may include memories. Figure 13 The electronic device 50 shown should not impose any limitations on the functions and usage scope of the embodiments of the present disclosure.

[0367] As Figure 13 shown, the electronic device 50 may be presented in the form of a general-purpose computing device. For example, it may be a server device. The components of the electronic device 50 may include, but are not limited to: at least one of the above processors 51, at least one of the above memories 52, and a bus 53 connecting different system components (including the memory 52 and the processor 51).

[0368] The bus 53 includes a data bus, an address bus, and a control bus.

[0369] The memory 52 may include volatile memory, such as random access memory (RAM) 521 and / or cache memory 522, and may further include read-only memory (ROM) 523.

[0370] The memory 52 may further include a program tool 525 (or utility) having a set (at least one) of program modules 524. Such program modules 524 include, but are not limited to: an operating system, one or more application programs, other program modules, and program data. Each or some combination of these examples may include the implementation of a network environment.

[0371] The processor 51 executes various functional applications and data processing by running the computer program stored in the memory 52, such as the method provided in any of the above embodiments.

[0372] The electronic device 50 can also communicate with one or more external devices 54 (such as a keyboard, a pointing device, etc.). Such communication can be carried out through the input / output (I / O) interface 55. Moreover, the electronic device 50 can also communicate with one or more networks (such as a local area network (LAN), a wide area network (WAN) and / or a public network, such as the Internet) through the network adapter 56. As shown in the figure, the network adapter 56 communicates with other modules of the electronic device 50 through the bus 53. It should be understood that although not shown in the figure, other hardware and / or software modules can be used in combination with the electronic device 50, including but not limited to: microcode, device drivers, redundant processors, external disk drive arrays, RAID (redundant array of independent disks) systems, magnetic tape drives, and data backup storage systems, etc.

[0373] It should be noted that although several units / modules or sub-units / modules of the electronic device are mentioned in the above detailed description, this division is merely exemplary and not mandatory. In fact, according to the embodiments of the present disclosure, the features and functions of two or more units / modules described above can be embodied in one unit / modules. Conversely, the features and functions of one unit / modules described above can be further divided and embodied by multiple units / modules.

[0374] Embodiment 4

[0375] Some embodiments of the present disclosure also provide a computer-readable storage medium, on which a computer program is stored, and when the program is executed by a processor, the method provided by any of the above embodiments is implemented.

[0376] Among them, the computer-readable storage medium can more specifically include but not limited to: a portable disk, a hard disk, a random access memory, a read-only memory, an erasable programmable read-only memory, an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0377] Embodiment 5

[0378] Some embodiments of the present disclosure also provide a computer program product, including a computer program, and when the computer program is executed by a processor, the method of any of the above is implemented.

[0379] Among them, the program code for executing the computer program product of the present disclosure can be written in any combination of one or more programming languages, and the program code can be executed entirely on the user device, partially on the user device, executed as an independent software package, partially on the user device and partially on a remote device, or entirely on a remote device.

[0380] Although the specific embodiments of the present invention have been described above, those skilled in the art should understand that this is only an example, and the protection scope of the present invention is defined by the appended claims. Without departing from the principle and essence of the present invention, those skilled in the art can make various changes or modifications to these embodiments, but these changes and modifications all fall within the protection scope of the present invention.

Claims

1. A control method for a robot, the robot including a base, at least one link, and joints connecting the at least one link and the base, the at least one link including an end effector located at the end of the robot, characterized in that, The control method includes: Obtaining the information to be processed, where the information to be processed includes the first sphere representation information of the robot, the second sphere representation information of the target object, and the information of the forward kinematic function of the robot; Based on the information to be processed, obtaining the desired acceleration of each joint of the robot through the Riemannian motion strategy; Generating a control instruction according to the desired acceleration; Sending the control instruction to the robot to control the robot to reach the specified position.

2. The control method according to claim 1, wherein the information to be processed further includes the third sphere representation information corresponding to the floor or the wall.

3. The control method according to claim 1, wherein the information to be processed further includes at least one of the real-time operation parameter information of the robot, the target position information of the end effector, and the translation parameter of the base.

4. The control method according to claim 1, wherein the Riemannian motion strategy includes a number of subtasks, and the subtasks include at least one of the following: target attractor subtask, collision avoidance subtask, joint limit subtask, joint speed upper limit subtask, end effector attitude alignment subtask, end effector normal vector alignment subtask.

5. The control method according to claim 4, wherein the input parameters of the target attractor subtask include the second sphere representation information corresponding to the target position on the target object to be approached, the differentiable forward kinematic function of the robot, and the current joint angles and speeds of the robot; the Riemannian metric and the acceleration value corresponding to the target attractor subtask are calculated according to the normalized distance or the squared distance between the target position and the current position of the end effector.

6. The control method according to claim 4, wherein the input parameters of the collision avoidance subtask include a second sphere set representing one or more target objects in the target scene, a first sphere set representing one or more robot components, the differentiable forward kinematic function of the robot, and the current joint angles and speeds of the robot; the Riemannian metric and the acceleration value corresponding to the collision avoidance subtask are calculated according to the total sum of the soft collision cost functions of all sphere pairs, wherein the total sum of the soft collision cost functions is calculated according to the pairwise distance between the first sphere in the first sphere set and the second sphere in the second sphere set.

7. The control method according to claim 4, wherein the input parameters of the end effector attitude alignment subtask include the target world coordinates of the end effector, the differentiable forward kinematic function of the robot, and the current joint angles and speeds of the robot; the Riemannian metric and the acceleration value corresponding to the end effector attitude alignment subtask are calculated according to the difference between the current normal and tangent vectors of the end effector and the desired normal and tangent vectors.

8. The control method according to any one of claims 1-7, characterized in that, The control method further includes: Based on the information to be processed, obtaining the additional acceleration of each joint of the robot through a pre-trained neural network; Specifically, generating the control instruction according to the desired acceleration includes: Superposing the desired acceleration and the additional acceleration to obtain the joint acceleration of each joint of the robot; Generating a control instruction according to the joint acceleration.

9. The control method according to claim 8, wherein The calculation formula of the joint acceleration is as follows: a joint = a base + w * a residual ; where a joint represents the combined acceleration, a base represents the desired acceleration, a residual represents the additional acceleration, and w represents a positive weight.

10. The control method according to claim 8, wherein The input of the neural network includes the distance vector from the robot to the obstacle, the three-dimensional pose difference vector between the current position and pose of the end effector and the target position and pose, and the customized reward for motivating good behavior; and / or, The neural network is a permutation-invariant neural network.

11. The control method according to claim 8, characterized in that The training process of the neural network specifically includes: Obtaining input parameters; Wherein, the input parameters include a second sphere set representing one or more target objects in a static pose in the target scenario, a first sphere set representing one or more robot components in a static pose, the differentiable forward kinematic function of the robot, the current joint angles and speeds of the robot, the initial 6D poses of all target objects and the robot base, and the target world coordinates of the robot end effector; Calculating the positions of the first sphere corresponding to the robot and the second sphere corresponding to the target object at the current moment; Iteratively generating the control instruction of the robot, and using the generated control instruction and the current joint angles and speeds of the robot to iteratively update the next set of joint angles and speeds of the robot at the next moment; Iteratively updating the relevant parameters of the neural network according to the simulation trajectory result of the robot until the training of the neural network is completed.

12. The control method according to claim 1, wherein The control method is implemented based on GPU.

13. The control method according to claim 12, wherein, The control method further includes: Performing operations using a batch processing operator for parallel operation on the GPU; and / or, Pre-compiling and warming up the TensorFlow computation graph of the Riemannian motion strategy in the initialization stage, and using the pre-compiled TensorFlow computation graph to perform the calculation of the joint acceleration in the reinforcement learning process of the Riemannian motion strategy.

14. A control system for a robot, characterized in that, The control system includes: one or more processors for implementing the control method of the robot according to any one of claims 1-13.

15. An electronic device, comprising one or more memories, one or more processors, and a computer program stored on the one or more memories and adapted to run on the one or more processors, characterized in that When the one or more processors execute the computer program, the control method of the robot according to any one of claims 1-13 is implemented.

16. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by one or more processors, the control method of the robot according to any one of claims 1-13 is implemented.

17. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by one or more processors, the control method of the robot according to any one of claims 1-13 is implemented.

Images