Main Content

Compute Joint Torques To Balance An Endpoint Force and Moment

Generate torques to balance an endpoint force acting on the end-effector body of a planar robot. To calculate the joint torques using various methods, use the geometricJacobian and inverseDynamics object functions for a rigidBodyTree robot model.

Initialize Robot

The twoJointRigidBodyTree robot is a 2-D planar robot. Joint configurations are output as column vectors.

twoJointRobot = twoJointRigidBodyTree("column");

Problem Setup

The endpoint force eeForce is a column vector with a combination of the linear force and moment acting on the end-effector body ("tool"). Note that this vector is expressed in the base coordinate frame and is shown below.

fx = 2; 
fy = 2;
fz = 0;
nx = 0;
ny = 0;
nz = 3;
eeForce = [nx;ny;nz;fx;fy;fz];
eeName = "tool";

Specify the joint configuration of the robot for the balancing torques.

q = [pi/3;pi/4];
Tee = getTransform(twoJointRobot,q,eeName);

Geometric Jacobian Method

Using the principle of virtual work [1], find the balancing torque using the geometricJacobian object function and multiplying the transpose of the Jacobian by the endpoint force vector.

J = geometricJacobian(twoJointRobot,q,eeName);
jointTorques = J' * eeForce;
fprintf("Joint torques using geometric Jacobian (Nm): [%.3g, %.3g]",jointTorques);
Joint torques using geometric Jacobian (Nm): [1.41, 1.78]

Inverse Dynamics for Spatially-Transformed Force

Using another method, calculate the balancing torque by computing the inverse dynamics with the endpoint force spatially transformed to the base frame.

Spatially transforming a wrench from the end-effector frame to the base frame means to exert a new wrench in a frame that happens to collocate with the base frame in space, but is still fixed to the end-effector body; this new wrench has the same effect as the original wrench exerted at the ee origin. In the figure below, fext and next are the endpoint linear force and moment respectively, and the feebaseand neebaseare the spatially transformed forces and moments, respectively. In the snippet below, fbase_ee is the spatially transformed wrench.

r = tform2trvec(Tee);
fbase_ee = [cross(r,[fx fy fz])' + [nx;ny;nz]; fx;fy;fz];
fext = -externalForce(twoJointRobot, eeName, fbase_ee);
jointTorques2 = inverseDynamics(twoJointRobot, q, [], [], fext);
fprintf("Joint torques using inverse dynamics (Nm): [%.3g, %.3g]",jointTorques2)
Joint torques using inverse dynamics (Nm): [1.41, 1.78]

Inverse Dynamics for End-Effector Force

Instead of spatially transforming the endpoint force to the base frame, use a third method by expressing the end-effector force in its own coordinate frame (fee_ee). Transform the moment and the linear force vectors into the end-effector coordinate frame. Then, specify that force and the current configuration to the externalForce function. Calculate the inverse dynamics from this force vector.

eeLinearForce = Tee \ [fx;fy;fz;0];
eeMoment = Tee \ [nx;ny;nz;0];
fee_ee = [eeMoment(1:3); eeLinearForce(1:3)];
fext = -externalForce(twoJointRobot,eeName,fee_ee,q);
jointTorques3 = inverseDynamics(twoJointRobot, q, [], [], fext);
fprintf("Joint torques using inverse dynamics (Nm): [%.3g, %.3g]",jointTorques3);
Joint torques using inverse dynamics (Nm): [1.41, 1.78]

References

[1]Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Differential kinematics and statics. Robotics: Modelling, Planning and Control, 105-160.

[2]Harry Asada, and John Leonard. 2.12 Introduction to Robotics. Fall 2005. Chapter 6 Massachusetts Institute of Technology: MIT OpenCourseWare, https://ocw.mit.edu. License: Creative Commons BY-NC-SA.