{"id":1182,"date":"2018-04-25T02:00:42","date_gmt":"2018-04-25T00:00:42","guid":{"rendered":"https:\/\/blogs.mathworks.com\/student-lounge\/?p=1182"},"modified":"2018-09-20T00:23:33","modified_gmt":"2018-09-19T22:23:33","slug":"robot-manipulation-part-2-dynamics-and-control","status":"publish","type":"post","link":"https:\/\/blogs.mathworks.com\/student-lounge\/2018\/04\/25\/robot-manipulation-part-2-dynamics-and-control\/","title":{"rendered":"Robot Manipulation, Part 2: Dynamics and Control"},"content":{"rendered":"<p><!--introduction--> In this blog post, <a href=\"https:\/\/www.mathworks.com\/matlabcentral\/profile\/authors\/3069683-sebastian-castro\">Sebastian Castro<\/a> will talk about robot manipulation with MATLAB and Simulink. The <a href=\"https:\/\/blogs.mathworks.com\/racing-lounge\/?p=1148&amp;preview=true\">previous part<\/a> discussed <strong>kinematics<\/strong> (if you have not read it, we recommend you do), while this part discusses <strong>dynamics.<\/strong> <!--\/introduction--><\/p>\n<p>&#8211; &#8211;<\/p>\n<h1>Introduction<\/h1>\n<p>To motivate the importance of low-level robot manipulator control, I want to introduce a couple of engineering archetypes.<\/p>\n<ul>\n<li><strong>Robot programmers<\/strong> usually start with a robot that has controllable joint or end effector positions. If you are a robot programmer, you are probably implementing motion planning algorithms and integrating the manipulator with other software components, such as perception and decision-making.<\/li>\n<li><strong>Robot designers<\/strong> have a goal of enabling robot programmers. If you are a robot designer, you need to deliver a manipulator that can safely and reliably accept joint or end effector commands. You will likely apply some of the control design techniques discussed in this post and implement these controllers on embedded systems.<\/li>\n<\/ul>\n<p>Of course, nothing is quite as rigidly separated in real life. Chances are that robot manufacturers will provide their own controllers, but may also decide to expose control parameters, options, or maybe even a direct interface to the actuator torques.<\/p>\n<h1>From Kinematics to Dynamics<\/h1>\n<p>To recap the previous part, kinematics maps the joint positions of a robot manipulator to the positions and orientation of a coordinate frame of interest \u2013 usually, the end effector. Dynamics, on the other hand, maps the required joint forces and torques to their position, velocity, and acceleration.<\/p>\n<p><img decoding=\"async\" loading=\"lazy\" width=\"1024\" height=\"284\" class=\"aligncenter size-large wp-image-1186\" src=\"https:\/\/blogs.mathworks.com\/racing-lounge\/files\/2018\/04\/kin_vs_dyn-1024x284.png\" alt=\"\" \/><\/p>\n<p>To move from kinematics to dynamics, we need more information about the manipulator\u2019s mechanics. Specifically, we need the following inertial properties:<\/p>\n<ul>\n<li><strong>Mass<\/strong>: <a href=\"https:\/\/en.wikipedia.org\/wiki\/Newton%27s_laws_of_motion#Newton's_second_law\">Newton\u2019s second law<\/a> relates mass to force and linear acceleration.<\/li>\n<li><strong>Inertia<\/strong>: This is a 3&#215;3 matrix, commonly called the <a href=\"https:\/\/en.wikipedia.org\/wiki\/Moment_of_inertia#Inertia_tensor\">inertia tensor<\/a>, relating torque and angular acceleration. Since this matrix is <a href=\"https:\/\/en.wikipedia.org\/wiki\/Skew-symmetric_matrix\">skew-symmetric<\/a>, it can be defined with 6 parameters:\n<ul>\n<li>3 diagonal elements, or <strong>moments of inertia<\/strong>, which relate torque about an axis with acceleration about that same axis.<\/li>\n<li>3 off-diagonal elements, or <strong>products of inertia<\/strong>, which relate torque about an axis with acceleration about the other two axes.<\/li>\n<\/ul>\n<\/li>\n<li><strong>Center of mass:<\/strong> If the center of mass is not located at the body coordinate frame we defined, we need to apply the <a href=\"https:\/\/en.wikipedia.org\/wiki\/Parallel_axis_theorem\">parallel-axis theorem<\/a> to convert the rotations about the center of mass to rotations about our coordinate frame of interest.<\/li>\n<\/ul>\n<p>Typically, you will import a <a href=\"https:\/\/www.mathworks.com\/help\/robotics\/ref\/robotics.rigidbodytree-class.html\">robotics.RigidBodyTree<\/a> from an existing manipulator description \u2013 for example, a URDF file. In this case, the inertial properties will be automatically placed in each <a href=\"https:\/\/www.mathworks.com\/help\/robotics\/ref\/robotics.rigidbody-class.html\">robotics.RigidBody<\/a> that comprises the tree.<\/p>\n<p><img decoding=\"async\" loading=\"lazy\" class=\"aligncenter wp-image-1196 size-full\" src=\"https:\/\/blogs.mathworks.com\/racing-lounge\/files\/2018\/04\/urdf_vs_rbt-e1523359880819.png\" alt=\"\" width=\"480\" height=\"451\" \/><\/p>\n<h1>Controlling Joint Forces and Torques<\/h1>\n<p>A robot manipulator controller can contain the following components.<\/p>\n<ul>\n<li><strong>Feedback:<\/strong> Uses desired and measured motion to compute joint inputs. This usually involves a control law that minimizes the error between the desired and measured motion.<\/li>\n<li><strong>Feedforward: <\/strong>Uses desired motion only to compute joint inputs. This often \u2013 but not necessarily \u2013 involves a model of the manipulator mechanics to calculate an open-loop input.<\/li>\n<\/ul>\n<p><img decoding=\"async\" loading=\"lazy\" class=\"aligncenter wp-image-1200 size-full\" src=\"https:\/\/blogs.mathworks.com\/racing-lounge\/files\/2018\/04\/generic_controller_arch-e1523359920870.png\" alt=\"\" width=\"641\" height=\"204\" \/><\/p>\n<p>&nbsp;<\/p>\n<p>In our video &#8220;Controlling Robot Manipulator Joints&#8221;, we explore two different examples of joint controllers, featuring the 4-DOF <a href=\"http:\/\/emanual.robotis.com\/docs\/en\/platform\/openmanipulator\/\">ROBOTIS OpenManipulator platform<\/a>. You can also download the example files from the <a href=\"https:\/\/www.mathworks.com\/matlabcentral\/fileexchange\/65316-matlab-and-simulink-robotics-arena--designing-robot-manipulator-algorithms\">MATLAB Central File Exchange<\/a>.<\/p>\n<p><strong>[Video]<\/strong> <a href=\"https:\/\/www.mathworks.com\/videos\/matlab-and-simulink-robotics-arena-controlling-robot-manipulator-joints-1521714030608.html\">MATLAB and Simulink Robotics Arena: Controlling Robot Manipulator Joints<\/a><\/p>\n<p><div class=\"row\"><div class=\"col-xs-12 containing-block\"><div class=\"bc-outer-container add_margin_20\"><videoplayer><div class=\"video-js-container\"><video data-video-id=\"5756153653001\" data-video-category=\"blog\" data-autostart=\"false\" data-account=\"62009828001\" data-omniture-account=\"mathwgbl\" data-player=\"rJ9XCz2Sx\" data-embed=\"default\" id=\"mathworks-brightcove-player\" class=\"video-js\" controls><\/video><script src=\"\/\/players.brightcove.net\/62009828001\/rJ9XCz2Sx_default\/index.min.js\"><\/script><script>if (typeof(playerLoaded) === 'undefined') {var playerLoaded = false;}(function isVideojsDefined() {if (typeof(videojs) !== 'undefined') {videojs(\"mathworks-brightcove-player\").on('loadedmetadata', function() {playerLoaded = true;});} else {setTimeout(isVideojsDefined, 10);}})();<\/script><\/div><\/videoplayer><\/div><\/div><\/div><\/p>\n<h2>Controller Example 1: Inverse Kinematics + Joint Space Controllers<\/h2>\n<p>First, <strong>inverse kinematics (IK)<\/strong> is used to convert the reference end effector position to a set of reference joint angles. The controller then operates exclusively in the <strong>configuration space<\/strong> \u2013 that is, on joint positions.<\/p>\n<ul>\n<li>The <strong>feedforward<\/strong> term uses inverse dynamics on our manipulator model. This calculates the required joint forces\/torques such that the manipulator follows the desired motion, as well as compensates for gravity.<\/li>\n<li>The <strong>feedback<\/strong> term uses PID control. Each joint (4 revolute joints + gripper) has independent controllers that minimizes the error between desired and measured motion.<\/li>\n<\/ul>\n<p><img decoding=\"async\" loading=\"lazy\" class=\"aligncenter wp-image-1202 size-full\" src=\"https:\/\/blogs.mathworks.com\/racing-lounge\/files\/2018\/04\/ctrl_schematic_1-e1523360218630.png\" alt=\"\" width=\"640\" height=\"258\" \/><\/p>\n<p>For smooth motion, we typically want a closed-form trajectory such as a curve equation. This is because inverse dynamics requires positions, velocities, and accelerations to calculate required joint forces\/torques. So, having a differentiable reference trajectory makes this much easier.<\/p>\n<p>Theoretically, inverse dynamics should be enough to control a robot arm. However, there are factors such as joint mechanics (stiffness, damping, friction, etc.), unmeasurable disturbances, sensor\/actuator noise, or even numerical error, that can easily impact the robustness of a fully open-loop controller. Therefore, an additional feedback compensator is always recommended.<\/p>\n<p>While the feedforward and feedback control portions are relatively easy to implement and computationally inexpensive, this controller structure relies on solving IK. As we discussed in the previous part, the <a href=\"https:\/\/www.mathworks.com\/help\/robotics\/ref\/robotics.inversekinematics-system-object.html\">Robotics System Toolbox implementation<\/a> uses a numerical solution and therefore can require significant computation. You can address this by providing a good initial guess (usually the previous measurement), limiting the maximum number of iterations, or switching to an analytical IK solution.<\/p>\n<h2>Controller Example 2: Task Space Controller<\/h2>\n<p>This second controller performs the control in the <strong>task space<\/strong> \u2013 that is, on the end effector positions and orientations. In addition, it avoids the need for inverse kinematics by using the <strong>geometric Jacobian.<\/strong><\/p>\n<p>The <a href=\"https:\/\/www.mathworks.com\/help\/robotics\/ref\/robotics.rigidbodytree.geometricjacobian.html\">geometric Jacobian<\/a> is a function of the robot configuration <strong>q<\/strong> (joint angles\/positions), which is why it is often denoted as <strong>J(q)<\/strong>. The Jacobian is a mapping from the joint velocities to the world velocities of a coordinate frame of interest. However, with a bit of math you can find that it also maps the joint forces\/torques to world forces\/torques. I found <a href=\"https:\/\/studywolf.wordpress.com\/2013\/09\/02\/robot-control-jacobians-velocity-and-force\/\">this blog post<\/a> to be a helpful reference.<\/p>\n<ul>\n<li>The <strong>feedforward<\/strong> term in this controller only does one thing: compensates for gravity.<\/li>\n<li>The <strong>feedback<\/strong> term performs PID control on the XYZ positions of the end effector (we ignore orientation here, but you really shouldn\u2019t!) to calculate desired forces at the end effector coordinate frame. Then, the Jacobian converts the control outputs to joint torques and forces.<\/li>\n<\/ul>\n<p><img decoding=\"async\" loading=\"lazy\" class=\"aligncenter wp-image-1204 size-full\" src=\"https:\/\/blogs.mathworks.com\/racing-lounge\/files\/2018\/04\/ctrl_schematic_2-e1523360244506.png\" alt=\"\" width=\"640\" height=\"200\" \/><\/p>\n<p>Below is a screenshot of the Simulink model for this example controller. Unlike the schematic above, the model contains other realistic artifacts such as filters, rate limiters, saturation, and a decoupled gripper controller with basic logic. You\u00a0can download this model from the\u00a0<a href=\"https:\/\/www.mathworks.com\/matlabcentral\/fileexchange\/65316-matlab-and-simulink-robotics-arena--designing-robot-manipulator-algorithms\">MATLAB Central File Exchange<\/a>.<img decoding=\"async\" loading=\"lazy\" width=\"1024\" height=\"422\" class=\"aligncenter size-large wp-image-1198\" src=\"https:\/\/blogs.mathworks.com\/racing-lounge\/files\/2018\/04\/ctrl_screenshot-1024x422.png\" alt=\"\" \/><\/p>\n<p>&nbsp;<\/p>\n<h1>Survey of Control Design Techniques<\/h1>\n<p>Once you have a model of your manipulator, there are many tools in MATLAB and Simulink that can help you design joint controllers. These include<\/p>\n<ul>\n<li><a href=\"https:\/\/www.mathworks.com\/help\/slcontrol\/gs\/automated-tuning-of-simulink-pid-controller-block.html\">PID Tuner<\/a> for single-input, single-output (SISO) compensators<\/li>\n<li><a href=\"https:\/\/www.mathworks.com\/help\/control\/ref\/controlsystemdesigner-app.html\">Control System Designer<\/a> and <a href=\"https:\/\/www.mathworks.com\/help\/control\/ref\/controlsystemtuner-app.html\">Control System Tuner<\/a> for multi-input, multi-output (MIMO) systems<\/li>\n<li><a href=\"https:\/\/www.mathworks.com\/help\/mpc\/ref\/mpcdesigner-app.html\">MPC Designer<\/a> for model-predictive controllers<\/li>\n<\/ul>\n<p><img decoding=\"async\" loading=\"lazy\" width=\"1024\" height=\"588\" class=\"aligncenter size-large wp-image-1206\" src=\"https:\/\/blogs.mathworks.com\/racing-lounge\/files\/2018\/04\/pid_tuner-1024x588.png\" alt=\"\" \/><\/p>\n<p style=\"text-align: center;\"><em>PID Tuner output on the \u201cshoulder\u201d joint of the ROBOTIS OpenManipulator model<\/em><\/p>\n<p>Traditional control design relies on <a href=\"https:\/\/www.mathworks.com\/help\/slcontrol\/ug\/linearize-simulink-model.html\">linearization<\/a>, or finding a linear approximate of a nonlinear model about a specific operating point \u2013 for example, the \u201chome\u201d, or equilibrium, position of the manipulator. A controller designed about an approximate linear region can become less effective, and potentially unstable, as the robot state deviates from that region.<\/p>\n<p>Nonlinear control techniques can address this issue by considering the measured state of the system (in our case, joint or end effector positions). Feedforward techniques like inverse dynamics, or calculating the geometric Jacobian on the fly, can ensure that the controller accounts for nonlinearities in the model. Another popular technique is gain scheduling, which can be used for both <a href=\"https:\/\/www.mathworks.com\/help\/control\/gain-scheduled-controller-tuning.html\">traditional controllers<\/a> and <a href=\"https:\/\/www.mathworks.com\/help\/mpc\/gain-scheduling-mpc-design.html\">MPC controllers<\/a>.<\/p>\n<p>Another alternative is to employ model-free techniques such as:<\/p>\n<ul>\n<li><strong>Optimization:<\/strong> You can optimize control parameters using simulation, which is enabled by <a href=\"https:\/\/www.mathworks.com\/products\/sl-design-optimization.html\">Simulink Design Optimization<\/a>. While optimization makes no guarantees on stability, it lets you automatically tune a breadth of parameters such as gains, control effort\/rate limits, thresholds, etc. which could lead to good results \u2013 especially on highly nonlinear systems.<\/li>\n<li><strong>Machine learning:<\/strong> Reinforcement learning, or automatically learning by trial and error, is a common technique being employed for robot manipulation. For example, this <a href=\"https:\/\/arxiv.org\/pdf\/1603.02199.pdf\">paper<\/a> and <a href=\"https:\/\/youtu.be\/cXaic_k80uM\">video<\/a>\u00a0shows deep reinforcement learning \u2013 in other words, learning parameters of a deep neural network using reinforcement learning techniques.<\/li>\n<\/ul>\n<h1>Conclusion<\/h1>\n<p>Now you\u2019ve seen an overview of kinematics and dynamics for robot manipulator design. I hope this was a useful introduction to the language in this domain, some common techniques used in practice, and areas where MATLAB and Simulink can help you design and control robots.<\/p>\n<p>We hope that Simulink can help you during your design phase as you explore different architectures, integrate supervisory logic, perform tradeoff studies, and more. Also, recall that Simulink lets you <a href=\"https:\/\/www.mathworks.com\/solutions\/embedded-code-generation.html\">automatically generate standalone C\/C++ code<\/a> from your control algorithms, so they can be deployed to <a href=\"https:\/\/www.mathworks.com\/hardware-support\/home.html\">hardware<\/a> or middleware such as <a href=\"http:\/\/www.ros.org\/\">ROS<\/a>.<\/p>\n<p>If you want to see more material on robot manipulation, or other topics in robotics, feel free to leave us a comment or email us at <a href=\"mailto:roboticsarena@mathworks.com\">roboticsarena@mathworks.com<\/a>. I hope you enjoyed reading!<\/p>\n<p>&#8211; Sebastian<\/p>\n","protected":false},"excerpt":{"rendered":"<div class=\"thumbnail thumbnail_asset asset_overlay video\"><a href=\"https:\/\/blogs.mathworks.com\/student-lounge\/2018\/04\/25\/robot-manipulation-part-2-dynamics-and-control\/?dir=autoplay\"><img decoding=\"async\" src=\"https:\/\/cf-images.us-east-1.prod.boltdns.net\/v1\/static\/62009828001\/363d5a32-2c56-4abe-92a9-8009de524bb2\/0cd2118e-b2ce-4a8a-9c09-eb81de3fcdeb\/1280x720\/match\/image.jpg\" onError=\"this.style.display ='none';\"\/><\/p>\n<div class=\"overlay_container\">\n      <span class=\"icon-video icon_color_null\"><time class=\"video_length\">24:44<\/time><\/span>\n      <\/div>\n<p>      <\/a><\/div>\n<p> In this blog post, Sebastian Castro will talk about robot manipulation with MATLAB and Simulink. The previous part discussed kinematics (if you have not read it, we recommend you do), while this&#8230; <a class=\"read-more\" href=\"https:\/\/blogs.mathworks.com\/student-lounge\/2018\/04\/25\/robot-manipulation-part-2-dynamics-and-control\/\">read more >><\/a><\/p>\n","protected":false},"author":155,"featured_media":1200,"comment_status":"open","ping_status":"closed","sticky":false,"template":"","format":"standard","meta":[],"categories":[14,9,8],"tags":[20,100,96,94],"_links":{"self":[{"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/posts\/1182"}],"collection":[{"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/users\/155"}],"replies":[{"embeddable":true,"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/comments?post=1182"}],"version-history":[{"count":10,"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/posts\/1182\/revisions"}],"predecessor-version":[{"id":1258,"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/posts\/1182\/revisions\/1258"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/media\/1200"}],"wp:attachment":[{"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/media?parent=1182"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/categories?post=1182"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/blogs.mathworks.com\/student-lounge\/wp-json\/wp\/v2\/tags?post=1182"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}