Robot¶
The RobotArm
class holds all information about the robot kinematics, its operating limits, its link and end-effector geometries. It allows to calculate the forward or inverse kinematics between joint and Cartesian taks space. A list of all supported robots is given at the end of this document.
Limits¶
By default, Jacobi will use robot limits specified by the manufacturer - or reasonable values in case noen are known publicly. You can optionally overwrite the joint limits via
robot.max_position = [2.97, 2.53, 3.72, 3.32, 2.18, 6.28] # [rad]
robot.min_position = [-2.97, -1.75, -1.22, -3.32, -2.18, -6.28] # [rad]
robot.max_velocity = [6.46, 5.41, 7.16, 9.60, 9.51, 17.45] # [rad/s]
robot.max_acceleration = [5.0, 5.0, 5.0, 10.0, 10.0, 12.0] # [rad/s^2]
robot.max_jerk = [1e3, 1e3, 1e3, 1e3, 1e3, 1e3] # [rad/s^3]
robot.max_position = {2.97, 2.53, 3.72, 3.32, 2.18, 6.28}; // [rad]
robot.min_position = {-2.97, -1.75, -1.22, -3.32, -2.18, -6.28}; // [rad]
robot.max_velocity = {6.46, 5.41, 7.16, 9.60, 9.51, 17.45}; // [rad/s]
robot.max_acceleration = {5.0, 5.0, 5.0, 10.0, 10.0, 12.0}; // [rad/s^2]
robot.max_jerk = {1e3, 1e3, 1e3, 1e3, 1e3, 1e3}; // [rad/s^3]
to restrict or slow down calculated motions for your application. Jacobi also supports optional minimum velocity and acceleration limits (min_velocity
and min_acceleration
) for asymmetric constraints.
Frames¶
Geometric poses and transformations are represented by the Frame
class. It can be constructed in several ways, including from translation only, from Euler angles, from Quaternions, or from affine transformation matrices.
grasp_pose = Frame(x=0.5, y=0.2, z=0.75) # [m]
# Euler angles (intrinsic ZYX convention)
grasp_pose = Frame.from_euler(x=0.5, y=0.2, z=0.75, a=0.0, b=0.0, c=0.0) # [m] [rad]
# Quaternion (w, x, y, z convention)
grasp_pose = Frame.from_quaternion(x=0.5, y=0.2, z=0.75, qw=1.0, qx=0.0, qy=0.0, qz=0.0)
# Either flattened 16 or 4x4 affine matrix
grasp_pose = Frame.from_matrix([0.8656, ..., 1.0])
auto grasp_pose = Frame::from_translation(0.5, 0.2, 0.75); // (x, y, z)[m]
// Euler angles (intrinsic ZYX convention)
auto grasp_pose = Frame::from_euler(0.5, 0.2, 0.75, 0.0, 0.0, 0.0);
// Quaternion (w, x, y, z convention)
auto grasp_pose = Frame::from_quaternion(0.5, 0.2, 0.75, 1.0, 0.0, 0.0, 0.0);
// Either flattened 16 or 4x4 affine matrix
auto grasp_pose = Frame::from_matrix({0.8656, ..., 1.0});
Frames support a range of features that you’d expect: They can be multiplied or inversed via .inverse()
, you can get the translational or rotational part via .translation
or .rotation
, and you can interpolate between two frames via a.interpolate(b, t=0.5)
.
End-Effector¶
The end-effector (e.g. a gripper or other custom tooling) is attached to the robot’s flange. It can be specified by an obstacle for collision checking and the transformation between the robot’s flange and the new tool center point (TCP):
robot.end_effector_obstacle = Convex.load_from_file(project_path / 'ee_hull.obj')
robot.flange_to_tcp = Frame.from_euler(0, 0, 0, np.pi, 0, np.pi/2) # [m, rad]
robot.end_effector_obstacle = Convex::load_from_file(project_path / "ee_hull.obj");
robot.flange_to_tcp = Frame::from_euler(0, 0, 0, np.pi, 0, np.pi/2); // [m, rad]
Note that an end-effector is optional. By default, the flange is used interchangbly for the robot’s TCP.
Item Obstacle¶
An item obstacle (e.g. for a grasped item) can be attached easily at the robot’s TCP. For both the item_obstacle
, as well as the end_effector_obstacle
, we can either set an Obstacle
robot.item_obstacle = Obstacle(object=Box(0.2, 0.2, 0.1), origin=Frame(z=0.05)) # [m]
robot.item_obstacle = Obstacle {Box {0.2, 0.2, 0.1}, Frame::from_translation(0.0, 0.0, 0.05)}; // object size, origin [m]
or an Object without any offset.
Forward & Inverse Kinematics¶
In case the robot’s base is not located at the environment origin, please update the pose first by setting
robot.base = Frame.from_translation(0.0, 0.0, 0.5) # [m]
robot.base = Frame::from_translation(0.0, 0.0, 0.5); // [m]
The forward kinematics maps the robot’s joint position to the Cartesian end-effector pose. In this regard, the tool center point (TCP) of the robot can be calculated via
tcp_frame = robot.calculate_tcp([1.0, 2.0, 0.2, 0.1, 0.0, 0.0]) # [rad]
Frame tcp_frame = robot.calculate_tcp([1.0, 2.0, 0.2, 0.1, 0.0, 0.0]); // [rad]
Note that the TCP is equalivalent to the robot’s flange in case no end-effector was specified.
The inverse kinematics (IK) maps the Cartesian end-effector to a joint position and is, in general, not unique and computationally much more complex. Jacobi supports an analytical IK for all 6-DoF robot arms adn a numerical IK for all redundant robots (e.g. the Kuka iiwa7 or the Franka Emika robot). Without any additional information, Jacobi’s inverse kinematics will select the best solution (satisfying the joint position limits) in case multiple exists and will return an empty solution if none exist.
grasp_pose = Frame.from_euler(0.5, 0.0, 0.75, 0.393, 0.785, 0.0) # [m] [rad]
new_position = robot.inverse_kinematics(grasp_pose) # Might return None
grasp_pose = Frame.from_euler(0.5, 0.0, 0.75, 0.393, 0.785, 0.0); // [m] [rad]
std::optional<Config<UniversalUR10>> new_position = robot.inverse_kinematics(grasp_pose);
You can specify an initial joint position to change the solution (in case multiple exist) via
robot.inverse_kinematics(grasp_pose, reference_config)
. The analytical IK will return the nearest solution (according to the joint space distance), while the numerical IK will use it as its initial solution for the iterative update step.
Custom Link Objects¶
Jacobi includes collision models of all supported robots. If you want to specify custom collision models for the robot links (so beside the end-effector, e.g. for cable housing), you can overwrite a default link via
robot.link_obstacles[2] = Convex.load_from_file(project_path / 'upper_with_backpack.obj')
robot.link_obstacles[2] = Convex::load_from_file(project_path / "upper_with_backpack.obj");
Please make sure that your link objects have their base in the corresponding link frame e.g. link_frames[2]
.
Supported Robots¶
Jacobi currently supports the following list of robots. Reach out to us on https://jacobirobotics.com/get-started to get your robot supported!
Manufacturer |
Robot Model |
Driver |
---|---|---|
ABB |
IRB 1200-5/90 |
ABBDriver |
IRB 1300-7/1.4 |
ABBDriver |
|
IRB 1600-6/1.2 |
ABBDriver |
|
IRB 2600-12/185 |
ABBDriver |
|
IRB 4600-60/205 |
ABBDriver |
|
IRB 6700-150/320 |
ABBDriver |
|
IRB 14000 YuMi |
ABBDriver |
|
FANUC |
CRX-30iA |
|
LR 10-iA/10 |
||
LRMate 200iD/7L |
||
M 20-iB/25 |
||
M 710-iC/45M |
||
Franka |
Panda |
|
Kinova |
Gen3 7DoF |
|
KUKA |
KR6 R700 sixx |
|
KR70 R2100 |
||
LBR iiwa 7 |
||
Mecademic |
Meca500 |
|
UFACTORY |
xArm 7 |
|
Universal |
UR5e |
UniversalDriver |
UR10 |
UniversalDriver |
|
UR10e |
UniversalDriver |
|
UR20 |
UniversalDriver |
|
Yaskawa |
GP12 |
YaskawaDriver |
GP180 |
YaskawaDriver |
|
GP180-120 |
YaskawaDriver |
|
GP50 |
YaskawaDriver |
|
HC10 |
YaskawaDriver |
|
HC20 |
YaskawaDriver |
Loading Custom Robots¶
Jacobi is robot agnostic. So if you do not see your desired robot in our robot library, you can use your own URDF
(Unified Robot Description Format) file that represents the robot. You must provide the name of the base link of your kinematic chain as well as the end link of your robot kinematic chain. Note that custom robots are currently in beta preview with rudimentary support. Please reach out to us if you encounter any issues.
Custom robot arms are loaded using the CustomRobot
class, which extends the RobotArm
class. It provides all methods and properties we described above, like forward and inverse kinematics, specifying robot limits and adding end-effector and item obstacles.
Copy code
robot = CustomRobot.load_from_urdf_file(
file='path/to/your_robot.urdf',
base_link='base_link_name',
end_link='flange_link_name'
)
auto robot = CustomRobot::load_from_urdf_file(
"path/to/your_robot.urdf",
"base_link_name",
"flange_link_name"
);
There are two requirements for the URDF file you want to load: all links require a single mesh file for collision checking, provided as a .obj
file, and all links need to be connected via joints from the base link to the end link.