HebiKinematics
This example shows howto use the HebiKinematics API to calculate forward kinematics, Jacobians, as well as forces and torques to compensate for accelerations due to gravity and the dynamics.
Contents
| Created | July 13, 2017 |
| Last Update | Aug 22, 2017 |
| API Version | hebi-matlab-1.0-rc2 |
| Requirements | MATLAB 2013b or higher |
Copyright 2017 HEBI Robotics
Define kinematic structure using HEBI components
The addBody method creates a serial chain of bodies that describe the kinematic relation of a robot. A body can be a rigid link or a dynamic element. The first body represents the base and the last body represents the end-effector.
The following code creates a representation of the rendered 5-DoF arm shown below. For more information on available bodies, please consult the help files and/or the online documentation at http://docs.hebi.us.
% Setup 5dof arm kin = HebiKinematics(); kin.addBody('X5-4'); % base joint kin.addBody('X5-HeavyBracket', 'mount', 'right-inside'); kin.addBody('X5-9'); kin.addBody('X5-Link', 'ext', 0.350, 'twist', pi); kin.addBody('X5-9'); kin.addBody('X5-Link', 'ext', 0.250, 'twist', pi); kin.addBody('X5-1'); kin.addBody('X5-LightBracket', 'mount', 'left'); kin.addBody('X5-1'); % Display display(kin);
kin =
<a href="matlab:helpPopup HebiKinematics">HebiKinematics</a> with properties:
numBodies: 9
numDoF: 5
mass: 2.7 [kg]
payload: 0 [kg]
body type isDoF mass
---- --------------- ----- -----
1 X5-4 true 0.335
2 X5-HeavyBracket false 0.215
3 X5-9 true 0.360
4 X5-Link false 0.400
5 X5-9 true 0.360
6 X5-Link false 0.300
7 X5-1 true 0.315
8 X5-LightBracket false 0.100
9 X5-1 true 0.315
Calculate Forward Kinematics
While the HebiKinematics and the HebiGroup APIs were desigend to work well together, the HebiKinematics API is independent and can be used by itself. To keep these examples simple, we use pre-defined position vectors. In a real application, positions can be set to the group feedback.
% Input position vector (could be replaced with "position = fbk.position") position = rand(1, kin.getNumDoF); display(position); % 4x4xN transforms from the base frame to each output frames = kin.getForwardKinematics('output', position); % 4x4 transform from the base frame to the end-effector endEffector = kin.getForwardKinematicsEndEffector(position); display(endEffector);
position =
0.6557 0.0357 0.8491 0.9340 0.6787
endEffector =
0.2296 -0.9686 -0.0953 0.4359
0.9688 0.2369 -0.0733 0.2741
0.0936 -0.0755 0.9927 -0.0096
0 0 0 1.0000
Calculate Jacobian
The calls to get the Jacobian work the same way as the forward kinematics.
% 6x numDoF x numBodies matrix J = kin.getJacobian('output', position); % 6 x numDoF matrix J_endEffector = kin.getJacobianEndEffector(position); display(J_endEffector);
J_endEffector =
-0.2741 0.0758 -0.0857 -0.0583 0
0.4359 0.0583 -0.0659 -0.0448 0
0 0.5126 -0.1628 -0.0089 0
0.0000 0.6097 -0.6097 0.6097 -0.0953
-0.0000 -0.7926 0.7926 -0.7926 -0.0733
1.0000 0.0000 0.0000 0.0000 0.9927
Gravity Compensation
There are many use cases, such as teach-repeat, that benefit from an arm being in a compliant 'zero-gravity' mode. The Kinematics API provides convenience methods that help with calculating the required forces and torques to counter gravity.
% Calculate compensatory torques/forces at position zero w/ gravity % pointing in the negative z direction gravityVec = [0 0 -1]; position = zeros(1, kin.getNumDoF); gravCompEfforts = kin.getGravCompEfforts(position, gravityVec);
The following example continuously compensates for gravity on a 2 dof arm. It works with any serial chain robot configuration provided that the actuators in the group match the kinematic configuration.
% Setup 2 dof planar RR arm (could be any number of DoF) kin = HebiKinematics(); kin.addBody('X5-4'); % base joint kin.addBody('X5-Link', 'ext', 0.35, 'twist', pi); kin.addBody('X5-1'); kin.addBody('X5-Link', 'ext', 0.25, 'twist', pi); % Setup the group that corresponds to the cnfiguration group = HebiLookup.newGroupFromNames('arm', {'base', 'shoulder'}); % Determine the direction of gravity based on the built-in IMU % (assumes fixed base) fbk = group.getNextFeedback(); gravityVec = -[fbk.accelX(1) fbk.accelY(1) fbk.accelZ(1)]; % Continuously command a 'weight-less' mode t0 = tic(); cmd = CommandStruct(); while toc(t0) < 5 fbk = group.getNextFeedback(); cmd.effort = kin.getGravCompEfforts(fbk.position, gravityVec); group.send(cmd); end
Dynamics Compensation
Similarly to gravity compensation, HebiKinematics also provides a way to calculate forces and torques to compensate for the joint accelerations due to dynamic motions.
This does require knowledge of positions, velocities, and accelerations at a given point, so it is primarily useful when being combined with a trajectory, such as provided by the HebiTrajectory API (later example).
The dynamics compensation does not include the torques/forces required to compensate for gravity. Thus, getDynamicCompEfforts() is typically used in combination with getGravCompEfforts().
% Create sample sinusoidal motion time = 0.2; freq = 1 * (2*pi); % 1 Hz amp = 1; allDoF = ones(1,group.getNumModules); % Sample point in trajectory cmdPosition = amp * sin( freq * time ) * allDoF; cmdVelocity = freq * amp * cos( freq * time ) * allDoF; cmdAcceleration = -freq^2 * amp * sin( freq * time ) * allDoF; % Calculate torques at sampled point within trajectory efforts = kin.getDynamicCompEfforts(... fbk.position, ... cmdPosition, ... cmdVelocity, ... cmdAcceleration); % Display display(cmdPosition); display(cmdVelocity); display(cmdAcceleration); display(efforts);
cmdPosition =
0.9511 0.9511
cmdVelocity =
1.9416 1.9416
cmdAcceleration =
-37.5462 -37.5462
efforts =
-3.7878 0.4938