C++¶
Geometry¶
-
using Config = std::vector<double>
Type alias for
std::vector<double>
as a joint configuration of a robot.
-
struct Frame : public Eigen::Isometry3d
Represents a transformation or pose in 3D Cartesian space.
Encapsulates translation and rotation in a unified manner for 3D poses.
Public Types
-
using Translation = Eigen::Translation3d
Translation component.
-
using EulerAngles = Eigen::EulerAngles<double, Eigen::EulerSystemXYZ>
Euler angles representation.
-
using EulerParameter = std::array<double, 6>
Euler angles and translation.
-
using QuaternionParameter = std::array<double, 7>
Quaternion and translation.
Public Functions
-
Frame() = default
Default constructor.
-
Frame(const Eigen::Isometry3d &frame)
Construct a Frame from an Eigen Isometry3d object.
- Parameters:
frame – An Eigen Isometry3d object representing the transformation.
-
double translational_distance(const Frame &other) const
Calculate the Euclidean distance between two Frames’ positions.
- Parameters:
other – The other Frame to compare against.
- Returns:
double The Euclidean distance.
-
double angular_distance(const Frame &other) const
Calculate the angular distance between two Frames’ orientations.
- Parameters:
other – The other Frame to compare against.
- Returns:
double The angular distance.
-
Frame interpolate(double t, const Frame &other) const
Interpolate between this Frame and another Frame.
- Parameters:
t – The interpolation parameter (0.0 to 1.0).
other – The other Frame to interpolate towards.
- Returns:
Frame The interpolated Frame.
-
std::array<double, 16> to_matrix() const
Convert the Frame to a 4x4 matrix.
- Returns:
std::array<double, 16> The Frame as a 4x4 matrix.
-
std::array<double, 6> to_euler() const
Convert the Frame to Euler angles and translation.
- Returns:
std::array<double, 6> The Frame as Euler angles and translation.
-
bool operator==(const Frame &other) const
Check if two Frames are equal.
- Parameters:
other – The other Frame to compare against.
- Returns:
bool True if the Frames are equal, false otherwise.
Public Static Functions
-
static Frame Identity()
Get the identity transformation.
- Returns:
Frame The identity Frame.
-
static Frame from_matrix(const std::array<double, 16> &data)
Create a Frame from a matrix.
- Parameters:
data – A 16-element array representing a 4x4 matrix.
- Returns:
Frame The Frame constructed from the matrix.
-
static Frame from_translation(double x, double y, double z)
Create a Frame from translation values.
- Parameters:
x – The x translation.
y – The y translation.
z – The z translation.
- Returns:
Frame The Frame with the specified translation.
-
static Frame from_quaternion(double x, double y, double z, double qw, double qx, double qy, double qz)
Create a Frame from quaternion values.
- Parameters:
x – The x component of the quaternion.
y – The y component of the quaternion.
z – The z component of the quaternion.
qw – The scalar component of the quaternion.
qx – The x component of the quaternion.
qy – The y component of the quaternion.
qz – The z component of the quaternion.
- Returns:
Frame The Frame with the specified quaternion.
-
static Frame from_euler(double x, double y, double z, double a, double b, double c)
Create a Frame from Euler angles.
- Parameters:
x – The x translation.
y – The y translation.
z – The z translation.
a – The rotation angle around the x-axis.
b – The rotation angle around the y-axis.
c – The rotation angle around the z-axis.
- Returns:
Frame The Frame with the specified translation and Euler angles.
-
static Frame from_two_vectors(std::array<double, 3> v1, std::array<double, 3> v2)
Create a Frame representing a rotation that brings two vectors into alignment.
- Parameters:
v1 – The first vector.
v2 – The second vector.
- Returns:
Frame The Frame with the rotation between the two vectors.
-
static Frame x(double x)
Create a Frame representing a translation along the x-axis.
- Parameters:
x – The translation value along the x-axis.
- Returns:
Frame The Frame with the x-axis translation.
-
static Frame y(double y)
Create a Frame representing a translation along the y-axis.
- Parameters:
y – The translation value along the y-axis.
- Returns:
Frame The Frame with the y-axis translation.
-
static Frame z(double z)
Create a Frame representing a translation along the z-axis.
- Parameters:
z – The translation value along the z-axis.
- Returns:
Frame The Frame with the z-axis translation.
-
static Frame Rx(double c, double s)
Create a Frame representing a rotation around the x-axis.
- Parameters:
c – The cosine of the rotation angle.
s – The sine of the rotation angle.
- Returns:
Frame The Frame with the rotation around the x-axis.
-
static Frame Ry(double c, double s)
Create a Frame representing a rotation around the y-axis.
- Parameters:
c – The cosine of the rotation angle.
s – The sine of the rotation angle.
- Returns:
Frame The Frame with the rotation around the y-axis.
-
static Frame Rz(double c, double s)
Create a Frame representing a rotation around the z-axis.
- Parameters:
c – The cosine of the rotation angle.
s – The sine of the rotation angle.
- Returns:
Frame The Frame with the rotation around the z-axis.
-
static Frame Rx(double a)
Create a Frame representing a rotation around the x-axis.
- Parameters:
a – The rotation angle around the x-axis.
- Returns:
Frame The Frame with the rotation around the x-axis.
-
static Frame Ry(double a)
Create a Frame representing a rotation around the y-axis.
- Parameters:
a – The rotation angle around the y-axis.
- Returns:
Frame The Frame with the rotation around the y-axis.
-
static Frame Rz(double a)
Create a Frame representing a rotation around the z-axis.
- Parameters:
a – The rotation angle around the z-axis.
- Returns:
Frame The Frame with the rotation around the z-axis.
-
using Translation = Eigen::Translation3d
-
struct Twist : public Eigen::Vector<double, 6>
Represents a velocity in 3D Cartesian space.
The Twist struct represents a 6-dimensional vector used to describe velocity in 3D Cartesian space. It consists of linear velocities in the x, y, and z directions, and angular velocities around these axes.
Public Functions
-
Twist()
Default constructor.
-
Twist(double x, double y, double z, double rx, double ry, double rz)
Construct a Twist with specified linear and angular velocities.
- Parameters:
x – The linear velocity along the x-axis.
y – The linear velocity along the y-axis.
z – The linear velocity along the z-axis.
rx – The angular velocity around the x-axis.
ry – The angular velocity around the y-axis.
rz – The angular velocity around the z-axis.
-
Twist(const std::array<double, 6> &twist)
Construct a Twist from an array.
- Parameters:
twist – An array containing the 6-dimensional twist values.
-
Twist(const Eigen::Vector<double, 6> &twist)
Construct a Twist from an Eigen vector.
- Parameters:
twist – An Eigen vector containing the 6-dimensional twist values.
-
Twist operator+(const Twist &other)
Add two Twist objects.
- Parameters:
other – The Twist object to add.
- Returns:
Twist The result of the addition.
-
Twist transform(const Eigen::Matrix3d &rotation, const Eigen::Vector3d &translation) const
Transform the Twist by a rotation and translation.
- Parameters:
rotation – A 3x3 rotation matrix.
translation – A 3D translation vector.
- Returns:
Twist The transformed Twist.
-
Twist cross(const Twist &other) const
Compute the cross product of two Twists.
- Parameters:
other – The Twist object to cross with this one.
- Returns:
Twist The result of the cross product.
-
std::array<double, 6> to_array() const
Convert the Twist to an array.
Converts the Twist object into a 6-element array, with the components arranged as [x, y, z, rx, ry, rz].
- Returns:
std::array<double, 6> The Twist represented as an array.
-
Twist()
Element¶
-
struct Element
The base element of a scene.
The Element struct represents the base element of a scene, such as a robot, camera, or obstacle. It contains a name, pose, and tags that can be used to identify and categorize the element.
Subclassed by jacobi::Camera, jacobi::CartesianRegion, jacobi::CartesianWaypoint, jacobi::Obstacle, jacobi::Region, jacobi::Robot, jacobi::Waypoint
Public Functions
-
bool has_tag(const std::string &tag) const
Checks whether a tag is present on the element. Tags are case-insensitive.
- Parameters:
tag – The tag to check for.
- Returns:
bool True if the tag is present, false otherwise.
-
std::optional<std::string> get_parameter(const std::string &tag) const
Reads the value of a tag parameter
param=value
. Tags are case-insensitive.- Parameters:
tag – The tag to read the parameter from.
- Returns:
std::optional<std::string> The value of the parameter if it exists, std::nullopt otherwise.
Public Members
-
std::string name
The unique name of the element, for display and identification.
-
Frame origin = {Frame::Identity()}
Pose of the element, relative to the parent. Is called “base” for robots in Studio.
-
std::vector<std::string> tags
Given tags of the element, might be with a parameter
param=value
.
-
bool has_tag(const std::string &tag) const
Waypoints¶
-
class Waypoint : public jacobi::Element
A joint-space waypoint with possible position, velocity, and/or acceleration values.
The Waypoint class represents a point in the joint space of a robot with associated position, velocity, and acceleration values.
Public Functions
-
Waypoint(std::initializer_list<double> data)
Construct a waypoint by position data.
- Parameters:
data – A list of position values to initialize the waypoint.
-
Waypoint(const Config &position)
Construct a waypoint with given position and zero velocity and acceleration.
- Parameters:
position – The joint position to initialize the waypoint.
-
explicit Waypoint(const Config &position, const Config &velocity)
Construct a waypoint with given position and velocity and zero acceleration.
- Parameters:
position – The joint position to initialize the waypoint.
velocity – The joint velocity to initialize the waypoint.
-
inline explicit Waypoint(const Config &position, const Config &velocity, const Config &acceleration)
Construct a waypoint with given position, velocity, and acceleration.
- Parameters:
position – The joint position to initialize the waypoint.
velocity – The joint velocity to initialize the waypoint.
acceleration – The joint acceleration to initialize the waypoint.
-
inline explicit Waypoint(size_t size)
Construct a zero-initialized waypoint with the given size.
- Parameters:
size – The degrees of freedom for the waypoint.
-
inline explicit Waypoint()
Construct an empty waypoint.
-
inline void append(const Waypoint &other)
Append another waypoint to this one.
This method concatenates the position, velocity, and acceleration data from another waypoint to this one.
- Parameters:
other – The waypoint to append to this one.
-
inline size_t size() const
Get the size (or degrees of freedom) of the waypoint.
- Returns:
size_t The degrees of freedom represented by the waypoint.
-
bool is_within(const Waypoint &other) const
Check if this waypoint is within another waypoint.
This method checks if the current waypoint’s position, velocity, and acceleration are within the respective boundaries of another waypoint.
- Parameters:
other – The waypoint to compare against.
- Returns:
true If this waypoint is within the boundaries of the other.
- Returns:
false If this waypoint is not within the boundaries of the other.
-
Waypoint(std::initializer_list<double> data)
-
class CartesianWaypoint : public jacobi::Element
A Cartesian-space waypoint with possible position, velocity, and/or acceleration values.
The CartesianWaypoint class represents a point in Cartesian space with associated position, velocity, and acceleration frames. It is used to define the robot state in Cartesian space at a specific instance, with optional reference joint positions for inverse kinematics.
Public Functions
-
CartesianWaypoint(const Frame &position, const std::optional<Config> &reference_config = std::nullopt)
Construct a Cartesian waypoint with given position and zero velocity and acceleration.
- Parameters:
position – The position frame to initialize the waypoint.
reference_config – An optional joint configuration for inverse kinematics.
-
explicit CartesianWaypoint(const Frame &position, const Frame &velocity, const std::optional<Config> &reference_config = std::nullopt)
Construct a Cartesian waypoint with given position and velocity and zero acceleration.
- Parameters:
position – The position frame to initialize the waypoint.
velocity – The velocity frame to initialize the waypoint.
reference_config – An optional joint configuration for inverse kinematics.
-
explicit CartesianWaypoint(const Frame &position, const Frame &velocity, const Frame &acceleration, const std::optional<Config> &reference_config = std::nullopt)
Construct a Cartesian waypoint with given position, velocity, and acceleration.
- Parameters:
position – The position frame to initialize the waypoint.
velocity – The velocity frame to initialize the waypoint.
acceleration – The acceleration frame to initialize the waypoint.
reference_config – An optional joint configuration for inverse kinematics.
-
explicit CartesianWaypoint() = default
Default constructor.
-
bool is_within(const CartesianWaypoint &other) const
Check if this Cartesian waypoint is within another Cartesian waypoint.
This method checks if the current waypoint’s position, velocity, and acceleration frames are within the respective boundaries of another CartesianWaypoint.
- Parameters:
other – The CartesianWaypoint to compare against.
- Returns:
true If this waypoint is within the boundaries of the other.
- Returns:
false If this waypoint is not within the boundaries of the other.
Check if this Cartesian waypoint is within a joint-space waypoint, given a specific robot arm.
This method checks if the current CartesianWaypoint’s position, velocity, and acceleration frames are within the boundaries of a joint space waypoint, considering the robot arm’s kinematics.
- Parameters:
other – The joint-space Waypoint to compare against.
robot – The robot arm associated with the joint-space waypoint.
- Returns:
true If this CartesianWaypoint is within the boundaries of the other joint-space waypoint.
- Returns:
false If this CartesianWaypoint is not within the boundaries of the other joint-space waypoint.
Public Members
-
std::optional<Config> reference_config
An optional joint position that is used as a reference for inverse kinematics.
-
CartesianWaypoint(const Frame &position, const std::optional<Config> &reference_config = std::nullopt)
-
class Region : public jacobi::Element
A joint-space region with possible position, velocity, and/or acceleration values.
The Region class defines a region in joint space with boundaries on position, velocity, and acceleration. It is used to specify an area within which a the robot end-effector operates. The class provides methods to construct the region with varying levels of detail and to check if a given waypoint falls within the region’s boundaries.
Public Functions
-
explicit Region()
Default constructor.
-
explicit Region(const Config &min_position, const Config &max_position)
Construct a Region with position boundaries.
- Parameters:
min_position – The minimum position boundary.
max_position – The maximum position boundary.
-
explicit Region(const Config &min_position, const Config &max_position, const Config &min_velocity, const Config &max_velocity)
Construct a Region with position and velocity boundaries.
- Parameters:
min_position – The minimum position boundary.
max_position – The maximum position boundary.
min_velocity – The minimum velocity boundary.
max_velocity – The maximum velocity boundary.
-
explicit Region(const Config &min_position, const Config &max_position, const Config &min_velocity, const Config &max_velocity, const Config &min_acceleration, const Config &max_acceleration)
Construct a Region with position, velocity, and acceleration boundaries.
- Parameters:
min_position – The minimum position boundary.
max_position – The maximum position boundary.
min_velocity – The minimum velocity boundary.
max_velocity – The maximum velocity boundary.
min_acceleration – The minimum acceleration boundary.
max_acceleration – The maximum acceleration boundary.
-
inline size_t size() const
Get the size (or degrees of freedom) of the region.
- Returns:
size_t The degrees of freedom represented by the region.
-
bool is_within(const Waypoint &other) const
Check if a given waypoint is within the region.
This method checks if the given waypoint’s position, velocity, and acceleration fall within the respective boundaries defined by the region.
- Parameters:
other – The waypoint to check.
- Returns:
true If the waypoint is within the region’s boundaries.
- Returns:
false If the waypoint is not within the region’s boundaries.
Public Members
-
Config min_position
Minimum position boundary of the region.
-
Config max_position
Maximum position boundary of the region.
-
Config min_velocity
Minimum velocity boundary of the region.
-
Config max_velocity
Maximum velocity boundary of the region.
-
Config min_acceleration
Minimum acceleration boundary of the region.
-
Config max_acceleration
Maximum acceleration boundary of the region.
-
explicit Region()
-
struct CartesianRegionBound
The min or max boundary of a Cartesian region.
The CartesianRegionBound struct represents the boundaries of a region in Cartesian space. It defines both positional (x, y, z) and orientational (gamma, alpha) limits. These boundaries are used to constrain the movement or state of an object within a specified region.
Public Functions
-
CartesianRegionBound() = default
Default constructor.
-
inline explicit CartesianRegionBound(double x, double y, double z, double gamma = 0.0, double alpha = 0.0)
Construct a CartesianRegionBound with specified values.
- Parameters:
x – The x-coordinate of the boundary.
y – The y-coordinate of the boundary.
z – The z-coordinate of the boundary.
gamma – The rotational component around the z-axis (yaw).
alpha – The rotational component around the x-axis (roll).
Public Members
-
double x = {0.0}
The x-coordinate of the boundary.
-
double y = {0.0}
The y-coordinate of the boundary.
-
double z = {0.0}
The z-coordinate of the boundary.
-
double gamma = {0.0}
The rotational component around the z-axis (yaw).
-
double alpha = {0.0}
The rotational component around the x-axis (roll).
-
CartesianRegionBound() = default
-
class CartesianRegion : public jacobi::Element
A Cartesian-space region with possible minimum and maximum position, velocity, and/or acceleration values.
The CartesianRegion class defines a region in Cartesian space with optional boundaries for position, velocity, and acceleration. It is used to describe constraints on a Cartesian region within which the robot end-effector operates. The region can also have an associated reference configuration.
Public Functions
-
CartesianRegion() = default
Default constructor.
-
explicit CartesianRegion(const Frame &origin)
Construct a CartesianRegion with a specified origin.
- Parameters:
origin – The frame of reference for the region.
-
explicit CartesianRegion(const CartesianRegionBound &min_position, const CartesianRegionBound &max_position, const std::optional<Config> &reference_config = std::nullopt)
Construct a CartesianRegion with position boundaries and an optional reference configuration.
- Parameters:
min_position – The minimum position boundary.
max_position – The maximum position boundary.
reference_config – An optional reference configuration.
-
explicit CartesianRegion(const CartesianRegionBound &min_position, const CartesianRegionBound &max_position, const CartesianRegionBound &min_velocity, const CartesianRegionBound &max_velocity, const std::optional<Config> &reference_config = std::nullopt)
Construct a CartesianRegion with position and velocity boundaries, and an optional reference configuration.
- Parameters:
min_position – The minimum position boundary.
max_position – The maximum position boundary.
min_velocity – The minimum velocity boundary.
max_velocity – The maximum velocity boundary.
reference_config – An optional reference configuration.
-
explicit CartesianRegion(const CartesianRegionBound &min_position, const CartesianRegionBound &max_position, const CartesianRegionBound &min_velocity, const CartesianRegionBound &max_velocity, const CartesianRegionBound &min_acceleration, const CartesianRegionBound &max_acceleration, const std::optional<Config> &reference_config = std::nullopt)
Construct a CartesianRegion with position, velocity, and acceleration boundaries, and an optional reference configuration.
- Parameters:
min_position – The minimum position boundary.
max_position – The maximum position boundary.
min_velocity – The minimum velocity boundary.
max_velocity – The maximum velocity boundary.
min_acceleration – The minimum acceleration boundary.
max_acceleration – The maximum acceleration boundary.
reference_config – An optional reference configuration.
-
bool is_within(const CartesianWaypoint &other) const
Check if a CartesianWaypoint is within the region.
- Parameters:
other – The CartesianWaypoint to check.
- Returns:
true If the waypoint is within the region.
- Returns:
false If the waypoint is not within the region.
Check if a Waypoint is within the region, given a specific robot arm.
- Parameters:
other – The Waypoint to check.
robot – The robot arm associated with the waypoint.
- Returns:
true If the waypoint is within the region.
- Returns:
false If the waypoint is not within the region.
Public Members
-
CartesianRegionBound min_position
Minimum position boundary of the region.
-
CartesianRegionBound max_position
Maximum position boundary of the region.
-
CartesianRegionBound min_velocity
Minimum velocity boundary of the region.
-
CartesianRegionBound max_velocity
Maximum velocity boundary of the region.
-
CartesianRegionBound min_acceleration
Minimum acceleration boundary of the region.
-
CartesianRegionBound max_acceleration
Maximum acceleration boundary of the region.
-
std::optional<Config> reference_config
Reference configuration for the region, if any.
-
CartesianRegion() = default
-
using MultiRobotPoint = std::map<std::shared_ptr<Robot>, std::variant<Config, Waypoint, CartesianWaypoint>>
A type for specifying an exact start or goal point per robot.
-
using Point = std::variant<Config, Waypoint, CartesianWaypoint, MultiRobotPoint, Region, CartesianRegion>
All types for specifying motion start or goals, either exact or by region.
-
using ExactPoint = std::variant<Config, Waypoint, CartesianWaypoint, MultiRobotPoint>
All types for specifying exact start or goal points.
Obstacles¶
-
struct Box
A box collision object.
The Box struct represents a simple 3D rectangular collision object. It is defined by its dimensions along the x, y, and z axes, which correspond to the width, depth, and height of the box, respectively.
Public Functions
-
inline explicit Box(float x, float y, float z)
Construct a box of size x, y, z along the respective axes.
- Parameters:
x – The width of the box along the x-axis.
y – The depth of the box along the y-axis.
z – The height of the box along the z-axis.
-
explicit Box() = default
Default constructor.
Public Members
-
float x
Dimensions of the box [m].
-
inline explicit Box(float x, float y, float z)
-
struct Capsule
A capsule collision object.
The Capsule struct represents a simple 3D collision object shaped like a capsule (a cylinder with hemispherical ends). It is defined by its radius and length along the z-axis. Capsules are commonly used in collision detection due to their computational efficiency.
Public Functions
-
inline explicit Capsule(float radius, float length)
Construct a capsule with the given radius and length.
- Parameters:
radius – The radius of the capsule.
length – The length of the capsule along the z-axis.
-
explicit Capsule() = default
Default constructor.
Public Members
-
float radius
Radius of the capsule [m].
-
float length
Length of the capsule along the z-axis [m].
-
inline explicit Capsule(float radius, float length)
-
struct Convex
A convex mesh collision object.
The Convex struct represents a 3D convex mesh, often used for collision detection. It supports loading from files, direct vertex and triangle specification, and provides utility functions like bounding box computation.
Public Functions
-
explicit Convex() = default
Default constructor.
-
explicit Convex(const std::vector<std::array<float, 3>> &verts, const std::vector<std::array<size_t, 3>> &triangs)
Construct a convex object from given vertices and triangles.
- Parameters:
verts – List of vertex positions.
triangs – List of triangles, each defined by three vertex indices.
-
std::array<double, 3> get_bounding_box_minimum() const
Get the minimum bounding box corner.
- Returns:
A 3D vector representing the minimum corner of the bounding box.
-
std::array<double, 3> get_bounding_box_maximum() const
Get the maximum bounding box corner.
- Returns:
A 3D vector representing the maximum corner of the bounding box.
Public Members
-
std::vector<Vector> vertices
List of vertices that define the convex mesh.
-
std::vector<Triangle> triangles
List of triangles that define the convex mesh surface.
Public Static Functions
-
static std::vector<Convex> load_from_file(const std::filesystem::path &path, std::optional<float> scale = std::nullopt)
- Deprecated:
Load convex objects from a file.
- Parameters:
path – The path to the file (*.obj).
scale – Optional scale to apply when loading the object.
- Returns:
A vector of Convex objects loaded from the file.
-
class Triangle
-
explicit Convex() = default
-
struct Cylinder
A cylinder collision object.
The Cylinder struct represents a 3D cylindrical collision object. It is defined by its radius and length along the z-axis.
Public Functions
-
inline explicit Cylinder(float radius, float length)
Construct a cylinder with the given radius and length.
- Parameters:
radius – The radius of the cylinder.
length – The length of the cylinder along the z-axis.
-
explicit Cylinder() = default
Default constructor.
Public Members
-
float radius
Radius of the cylinder [m].
-
float length
Length of the cylinder along the z-axis [m].
-
inline explicit Cylinder(float radius, float length)
-
struct DepthMap
A depth map collision object.
The DepthMap struct represents a 3D collision object based on a depth map, which is essentially a grid of depth values.
Public Functions
-
inline explicit DepthMap(const Matrix &depths, float x, float y)
Construct a depth map with the given data.
- Parameters:
depths – The matrix of depth values.
x – The size of the depth map along the x-axis.
y – The size of the depth map along the y-axis.
-
explicit DepthMap() = default
Default constructor.
Public Members
-
Matrix depths
Matrix containing the depth values at evenly spaced grid points.
-
float x
Size along the x-axis [m].
-
float y
Size along the y-axis [m].
-
float max_depth = {1e2}
Maximum depth to check for collisions [m].
This value sets the maximum depth that will be considered during collision checking. Any depth greater than this value will be ignored. The default value is 100 meters.
-
inline explicit DepthMap(const Matrix &depths, float x, float y)
-
struct MeshFile
A collision object loaded from a file.
Public Functions
-
explicit MeshFile() = default
Default constructor.
-
explicit MeshFile(const std::filesystem::path &path, std::optional<float> scale = std::nullopt)
Load a file obstacle.
- Parameters:
path – The path to the collision file (*.obj).
scale – Optional scale to apply when loading the object.
-
explicit MeshFile(const std::filesystem::path &visual_path, const std::filesystem::path &collision_path, std::optional<float> scale = std::nullopt)
Load a file obstacle.
- Parameters:
visual_path – The path to the visual file.
collision_path – The path to the collision file (*.obj).
scale – Optional scale to apply when loading the object.
Public Members
-
FileReference original_reference
The reference to the original file.
-
FileReference visual_reference
The reference to a visual file.
-
FileReference collision_reference
The reference to the collision file.
-
std::vector<Convex> collision
The convex obstacles for collision.
-
float scale = {1.0}
Scale of the object.
-
bool inside_project = {false}
Indicates if the file is part of a Jacobi Studio project. Then, the file content is not uploaded when referencing the file in Studio live.
Public Static Attributes
-
static std::filesystem::path base_path
Base path for file operations.
-
explicit MeshFile() = default
-
struct PointCloud
A point cloud collision object.
The PointCloud struct represents a 3D collision object based on a point cloud, which is a set of [x, y, z] points.
Public Functions
-
inline explicit PointCloud(const Matrix &points, double resolution = 0.01)
Construct a point cloud with the given data.
- Parameters:
points – The matrix (std::vector<std::array<float, 3>>) containing [x, y, z] points.
resolution – The resolution of the Octree that will be constructed from the point cloud and used for collision checking.
-
explicit PointCloud() = default
Default constructor.
Public Members
-
Matrix points
Matrix containing [x, y, z] points.
-
double resolution
The resolution of the Octree that will be constructed from the point cloud and used for collision checking.
-
inline explicit PointCloud(const Matrix &points, double resolution = 0.01)
-
struct Sphere
A sphere collision object.
The Sphere struct represents a 3D spherical collision object, defined by its radius, which determines its size in all directions.
Public Functions
-
inline explicit Sphere(float radius)
Construct a sphere with the given radius.
Initializes the sphere with the specified radius.
- Parameters:
radius – The radius of the sphere.
-
explicit Sphere() = default
Default constructor.
Public Members
-
float radius
Radius of the sphere [m].
-
inline explicit Sphere(float radius)
-
class Obstacle : public jacobi::Element
An environment obstacle.
The Obstacle class represents an object in an environment that can be used for both visualization and collision detection. Obstacles can have various geometric shapes and properties, and can be associated with a robot or be independent within the environment.
Public Functions
-
explicit Obstacle() = default
Default constructor.
-
Obstacle with_origin(const Frame &origin) const
Clone the current obstacle and set the new origin.
Creates a copy of the current obstacle and updates its origin to the provided frame. This function is useful for creating modified versions of an obstacle without altering the original object.
- Parameters:
origin – The new pose (origin) for the cloned obstacle.
- Returns:
Obstacle A new Obstacle instance with the updated origin.
-
Obstacle with_name(const std::string &name) const
Clone the current obstacle and update some parameters.
Creates a copy of the current obstacle and updates its name. This function is useful for creating modified versions of an obstacle without altering the original object.
- Parameters:
name – The new name for the cloned obstacle.
- Returns:
Obstacle A new Obstacle instance with the updated origin.
Public Members
-
Color color
The hex-string representation of the obstacle’s color, without the leading #.
This string defines the color of the obstacle for visualization purposes, formatted as a hex code (e.g., “FF5733” for orange).
-
std::optional<FileReference> visual
Optional reference to the visual file.
This optional reference points to an external visual file that represents the obstacle. If provided, this file will be used for rendering the obstacle’s appearance.
-
Geometry collision
The object for collision checking (and/or visualization).
This variant holds the geometric representation of the obstacle. It can be any of the supported shapes, including Box, Capsule, Convex, ConvexVector, Cylinder, DepthMap, PointCloud or Sphere.
-
bool for_visual = {true}
Whether this obstacle is used for visualization.
If true, the obstacle will be rendered in the environment’s visualization in Studio. By default, this is set to true.
-
bool for_collision = {true}
Whether this obstacle is used for collision checking.
If true, the obstacle will be considered in collision detection calculations. By default, this is set to true.
-
float safety_margin = {0.0}
An additional obstacle-specific safety margin for collision checking (on top of the environment’s global safety margin).
This margin adds an extra buffer around the obstacle during collision detection. It is specific to this obstacle and is added on top of any global safety margins.
-
Robot *robot = {nullptr}
Pointer to the associated robot, if any.
-
explicit Obstacle() = default
Environment¶
-
class Environment
The environment a robot (or multiple robots) lives in.
The Environment class manages static obstacles, collision checking, waypoints, cameras, and robots within a defined environment. The class provides methods to add, remove, and manage obstacles and waypoints, as well as to retrieve information about these elements. It also includes functionality for collision checking and updating the state of obstacles and robots.
Public Functions
Create an environment with a controllable robot.
- Parameters:
robot – The robot to add to the environment.
safety_margin – The global safety margin for collision checking [m].
Create an environment with multiple robots.
- Parameters:
robots – The robots to add to the environment.
safety_margin – The global safety margin for collision checking [m].
-
void set_safety_margin(float safety_margin)
Environment’s global safety margin for collision checking [m].
- Parameters:
global_safety_margin – The global safety margin for collision checking [m].
-
float safety_margin() const
Environment’s global safety margin for collision checking [m].
- Returns:
The global safety margin for collision checking [m].
-
std::shared_ptr<Robot> get_robot(const std::string &name = "") const
Get the robot with the given name from the environment.
In case there is only a single robot in the environment, the default empty name argument will return this robot. Otherwise throws an error if no robot with the name exists.
- Parameters:
name – The name of the robot to retrieve.
- Returns:
The shared pointer to a robot object associated with the given name.
-
std::vector<std::shared_ptr<Robot>> get_robots() const
Get all robots within the environment.
- Returns:
A vector of shared pointers to the robots within the environment.
-
Point get_waypoint(const std::string &name) const
Get the waypoint with the given name from the environment. Throws an error if no waypoint with the name exists.
- Parameters:
name – The name of the waypoint to retrieve.
- Returns:
The waypoint associated with the given name.
-
std::optional<Point> get_waypoint_by_tag(const std::string &tag) const
Get a waypoint within the environment given a tag. If multiple waypoints have the same tag, the first one to be found is returned.
- Parameters:
tag – The tag associated with the waypoint to retrieve.
- Returns:
An optional containing the waypoint associated with the given tag, or std::nullopt if no waypoint with the tag is found.
-
std::vector<Point> get_waypoints_by_tag(const std::string &tag) const
Get all waypoints within the environment given a tag.
- Parameters:
tag – The tag associated with the waypoints to retrieve.
- Returns:
A vector of waypoints that have the specified tag.
-
std::vector<Point> get_waypoints() const
Get all waypoints within the environment.
This function retrieves a list of all waypoints present in the environment.
- Returns:
A vector of waypoints within the environment.
-
std::shared_ptr<Obstacle> get_obstacle(const std::string &name) const
Get the obstacle with the given name from the environment. Throws an error if no obstacle with the name exists.
- Parameters:
name – The name of the obstacle to retrieve.
- Returns:
A shared pointer to the obstacle associated with the given name.
-
std::vector<std::shared_ptr<Obstacle>> get_obstacles_by_tag(const std::string &tag) const
Get all obstacles within the environment that carry the given tag.
- Parameters:
tag – The tag associated with the obstacles to retrieve.
- Returns:
A vector of shared pointers to the obstacles that have the specified tag.
-
std::vector<std::shared_ptr<Obstacle>> get_obstacles() const
Get all obstacles within the environment.
This function retrieves a list of all obstacles present in the environment.
- Returns:
A vector of shared pointers to the obstacles within the environment.
-
std::shared_ptr<Camera> get_camera(const std::string &name = "") const
Get a camera from the environment.
- Parameters:
name – Optional parameter to specify the name of the camera. If not provided, an empty string defaults to retrieving a camera without a specific name.
- Returns:
A shared pointer to the camera associated with the given name, or a default camera if the name is empty.
Add an obstacle to the environment (and returns the pointer to it)
- Parameters:
obstacle – The obstacle to add to the environment.
- Returns:
The original shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const Obstacle &obstacle)
Add an obstacle to the environment (and returns the pointer to it)
- Parameters:
obstacle – The obstacle to add to the environment.
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const Box &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a box-shaped obstacle to the environment.
- Parameters:
object – The box-shaped obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const Capsule &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a capsule-shaped obstacle to the environment.
- Parameters:
object – The capsule-shaped obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const Cylinder &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a cylinder-shaped obstacle to the environment.
- Parameters:
object – The cylinder-shaped obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const DepthMap &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a depth map-shaped obstacle to the environment.
- Parameters:
object – The depth map-shaped obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const MeshFile &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a file obstacle to the environment.
- Parameters:
object – The file obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const PointCloud &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a point cloud obstacle to the environment.
- Parameters:
object – The point cloud obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const Sphere &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a sphere-shaped obstacle to the environment.
- Parameters:
object – The sphere-shaped obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const Box &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add an obstacle with a name to the environment.
- Parameters:
name – The name to assign to the obstacle.
object – The obstacle to add (of a specific type, e.g., Box, Capsule, etc.).
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const Capsule &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Overload for adding an obstacle with a name and various shapes.
- Parameters:
name – The name to assign to the obstacle.
object – The obstacle to add (of a specific type, e.g., Capsule, Convex, etc.).
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const Cylinder &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a cylinder-shaped obstacle with a name to the environment.
- Parameters:
name – The name to assign to the obstacle.
object – The cylinder-shaped obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const DepthMap &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a depth map-shaped obstacle with a name to the environment.
- Parameters:
name – The name to assign to the obstacle.
object – The depth map-shaped obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const MeshFile &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a file obstacle with a name to the environment.
- Parameters:
name – The name to assign to the obstacle.
object – The file obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const PointCloud &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a point cloud obstacle with a name to the environment.
- Parameters:
name – The name to assign to the obstacle.
object – The point cloud obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
-
std::shared_ptr<Obstacle> add_obstacle(const std::string &name, const Sphere &object, const Frame &origin = Frame::Identity(), const Obstacle::Color &color = "000000", float safety_margin = 0.0)
Add a sphere-shaped obstacle with a name to the environment.
- Parameters:
name – The name to assign to the obstacle.
object – The sphere-shaped obstacle to add.
origin – The initial pose of the obstacle in the environment (default is Frame::Identity()).
color – The color of the obstacle in hexadecimal format (default is “000000”).
safety_margin – The safety margin around the obstacle (default is 0.0).
- Returns:
A shared pointer to the added obstacle.
Removes the given obstacles from the environment and from all collision checking.
- Parameters:
obstacle – The obstacle to remove from the environment.
-
void remove_obstacle(const std::string &name)
Removes all obstacles with the given name from the environment and from all collision checking.
- Parameters:
name – The name of the obstacle to remove from the environment.
Updates the given robot (or the default one if none was given) for the internal collision checking.
- Parameters:
robot – The robot to update the collision model for.
-
void update_fixed_obstacles()
Updates all fixed obstacles for the internal collision checking. This should be called after changing e.g. the position or size of an obstacle.
Updates the depths matrix of a given depth map obstacle for the internal collision checking.
- Parameters:
obstacle – The obstacle to update the depths map for.
Updates the point cloud of a given point cloud obstacle for the internal collision checking.
- Parameters:
obstacle – The point cloud to update the points for.
Updates the joint position of the given robot for the internal collision checking.
- Parameters:
robot – The robot to update the joint position for.
joint_position – The new joint position to set for the robot.
Check if a joint position is in collision.
- Parameters:
robot – The robot to check the joint position for.
joint_position – The joint position to check for collision.
detail – [out] The collision detail to fill in case of a collision.
- Returns:
True if the joint position is in collision, false otherwise.
Check if a joint position is in collision.
- Parameters:
robot – The robot to check the joint position for.
joint_position – The joint position to check for collision.
- Returns:
True if the joint position is in collision, false otherwise.
-
bool check_collision(const Config &joint_position)
Check if a joint position is in collision.
- Parameters:
joint_position – The joint position to check for collision.
- Returns:
True if the joint position is in collision, false otherwise.
Check if there exists a collision-free inverse kinematics for the Cartesian position.
- Parameters:
robot – The robot to check the Cartesian position for.
waypoint – The Cartesian position to check for collision.
- Returns:
True if there exists a collision-free inverse kinematics for the Cartesian position, false otherwise.
Check if there exists a collision-free inverse kinematics for the given robot and end-effector pose.
- Parameters:
robot – The robot to check for collision-free inverse kinematics.
tcp – The end-effector pose (transformation) to check for collision.
reference_config – Optional reference configuration for inverse kinematics of the end-effector pose.
- Returns:
True if there exists a collision-free inverse kinematics for the given end-effector pose, false otherwise.
-
bool check_collision(const CartesianWaypoint &waypoint)
Check if there exists a collision-free inverse kinematics for the Cartesian position.
- Parameters:
waypoint – The Cartesian position to check for collision.
- Returns:
True if there exists a collision-free inverse kinematics for the Cartesian position, false otherwise.
-
bool check_collision(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Check if there exists a collision-free inverse kinematics for the given end-effector pose.
- Parameters:
tcp – The end-effector pose to check for collision.
reference_config – Optional reference configuration for inverse kinematics of the end-effector pose.
- Returns:
True if there exists a collision-free inverse kinematics for the given end-effector pose, false otherwise.
Calculate a collision free joint position close to the reference position.
- Parameters:
joint_position – The reference joint position to find a collision free joint position close to.
robot – The robot to find a collision free joint position for.
- Returns:
An optional containing the collision free joint position close to the reference position, or std::nullopt if no such joint position exists.
Public Members
-
std::map<std::string, std::shared_ptr<Robot>> robots
All robots used for collision checking with default parameters.
-
std::map<std::string, Point> waypoints
All robots used for collision checking with default parameters.
-
std::vector<std::shared_ptr<Obstacle>> obstacles
All static obstacles in the environment, owning the data.
-
std::map<std::string, std::shared_ptr<Camera>> cameras
All cameras in the environment.
Public Static Functions
Carves (subtracts) an obstacle from a point cloud.
This function removes all points from the point cloud that are in collision with the obstacle. The point cloud is modified in place. The function does not update the collision model and update_point_cloud() may be called afterwards.
- Parameters:
point_cloud – [out] The point cloud to carve the obstacle from.
obstacle – The obstacle to carve from the point cloud.
Robots¶
-
class Robot : public jacobi::Element
The
Robot
class represents a robot arm or a collection of robot arms.The
Robot
class is an abstract class that represents a robot arm or a collection of robot arms. It provides a minimal interface for planning, including methods for forward kinematics and serialization.Subclassed by jacobi::RobotArm, jacobi::robots::DualArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const = 0
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual std::optional<double> get_control_rate() const = 0
Retrieves the control rate of the robot.
- Returns:
An optional double representing the control rate (in Hz) of the robot. If not available, the method will return std::nullopt.
-
virtual size_t get_degrees_of_freedom() const = 0
Retrieves the degrees of freedom of the robot.
- Returns:
The degrees of freedom (DOFs) or number of axes of the robot.
-
virtual Config get_position() const = 0
Retrieves the current position of the robot.
- Returns:
A
Config
object representing the current joint positions of the robot.
-
virtual Config get_min_position() const = 0
Retrieves the minimum position limits of the robot.
- Returns:
A
Config
object representing the minimum joint position limits of the robot.
-
virtual Config get_max_position() const = 0
Retrieves the maximum position limits of the robot.
- Returns:
A
Config
object representing the maximum joint position limits of the robot.
-
virtual Config get_max_velocity() const = 0
Retrieves the maximum velocity limits of the robot.
- Returns:
A
Config
object representing the maximum joint velocity limits of the robot.
-
virtual Config get_max_acceleration() const = 0
Retrieves the maximum acceleration limits of the robot.
- Returns:
A
Config
object representing the maximum joint acceleration limits of the robot.
-
virtual Config get_max_jerk() const = 0
Retrieves the maximum jerk limits of the robot.
- Returns:
A
Config
object representing the maximum joint jerk limits of the robot.
-
virtual void set_base(const Frame &base, const Frame &parent = Frame::Identity())
Sets the origin (base frame) of the robot.
- Parameters:
base – The
Frame
representing the new base or origin of the robot.parent – An optional
Frame
representing the parent frame. Defaults to the identity frame.
-
Frame base() const
The origin of the robot’s base.
- Returns:
A
Frame
representing the current origin of the robot base.
-
virtual void set_speed(double speed) = 0
Sets the velocity, acceleration, and jerk limits to a factor [0, 1] of their respective default (maximum) values.
- Parameters:
speed – A double representing the speed to be set for the robot.
Public Members
-
size_t id = {1024}
ID of the robot obstacles.
-
std::string model
The model name of the robot.
Public Static Functions
-
static std::shared_ptr<Robot> from_model(const std::string &model)
Returns the pre-defined robot with the given model name.
- Parameters:
model – A
std::string
representing the model specification.- Returns:
A
std::shared_ptr<Robot>
pointing to the createdRobot
.
-
virtual std::shared_ptr<Robot> clone() const = 0
-
class RobotArm : public jacobi::Robot, public std::enable_shared_from_this<RobotArm>
Subclassed by jacobi::robots::ABBGoFaCRB1500010, jacobi::robots::ABBIRB1200590, jacobi::robots::ABBIRB1200770, jacobi::robots::ABBIRB130010115, jacobi::robots::ABBIRB1300714, jacobi::robots::ABBIRB1600612, jacobi::robots::ABBIRB260012185, jacobi::robots::ABBIRB460060205, jacobi::robots::ABBIRB6640185280, jacobi::robots::ABBIRB6700150320, jacobi::robots::ABBYuMiIRB14000::Arm, jacobi::robots::CustomRobot, jacobi::robots::FanucCRX30iA, jacobi::robots::FanucLR10iA10, jacobi::robots::FanucLRMate200iD7L, jacobi::robots::FanucM20iB25, jacobi::robots::FanucM20iD25, jacobi::robots::FanucM710iC45M, jacobi::robots::FlexivRizon10, jacobi::robots::FlexivRizon10S, jacobi::robots::FlexivRizon4, jacobi::robots::FlexivRizon4S, jacobi::robots::FrankaPanda, jacobi::robots::KinovaGen37DoF, jacobi::robots::KukaIiwa7, jacobi::robots::KukaKR6R700sixx, jacobi::robots::KukaKR70R2100, jacobi::robots::MecademicMeca500, jacobi::robots::UfactoryXArm7, jacobi::robots::UniversalUR10, jacobi::robots::UniversalUR10e, jacobi::robots::UniversalUR20, jacobi::robots::UniversalUR5e, jacobi::robots::YaskawaGP12, jacobi::robots::YaskawaGP180, jacobi::robots::YaskawaGP180120, jacobi::robots::YaskawaGP50, jacobi::robots::YaskawaHC10, jacobi::robots::YaskawaHC20
Public Types
-
enum class InverseKinematicsMethod
The inverse kinematics method to use.
Values:
-
enumerator Analytic
Analytic inverse kinematics.
-
enumerator Numeric
Numeric inverse kinematics.
-
enumerator URAnalytic
Universal Robots Analytic inverse kinematics.
-
enumerator Analytic
-
using Jacobian = Eigen::Matrix<double, 6, Eigen::Dynamic>
The Jacobian matrix type for the robot arm.
Public Functions
-
inline virtual std::optional<double> get_control_rate() const override
Retrieves the control rate of the robot.
- Returns:
An optional double representing the control rate (in Hz) of the robot. If not available, the method will return std::nullopt.
-
inline virtual size_t get_degrees_of_freedom() const override
Retrieves the degrees of freedom of the robot.
- Returns:
The degrees of freedom (DOFs) or number of axes of the robot.
-
inline virtual Config get_position() const override
Retrieves the current position of the robot.
- Returns:
A
Config
object representing the current joint positions of the robot.
-
inline virtual Config get_min_position() const override
Retrieves the minimum position limits of the robot.
- Returns:
A
Config
object representing the minimum joint position limits of the robot.
-
inline virtual Config get_max_position() const override
Retrieves the maximum position limits of the robot.
- Returns:
A
Config
object representing the maximum joint position limits of the robot.
-
inline virtual Config get_max_velocity() const override
Retrieves the maximum velocity limits of the robot.
- Returns:
A
Config
object representing the maximum joint velocity limits of the robot.
-
inline virtual Config get_max_acceleration() const override
Retrieves the maximum acceleration limits of the robot.
- Returns:
A
Config
object representing the maximum joint acceleration limits of the robot.
-
inline virtual Config get_max_jerk() const override
Retrieves the maximum jerk limits of the robot.
- Returns:
A
Config
object representing the maximum joint jerk limits of the robot.
-
inline virtual std::optional<Obstacle> end_effector() const
Get the (optional) end effector attached to the robot’s flange.
- Returns:
An (optional) end effector attached to the robot’s flange.
-
virtual void set_end_effector(const std::optional<Obstacle> &end_effector)
Set the (optional) end effector attached to the robot’s flange.
- Parameters:
item – An (optional) end effector attached to the robot’s flange.
-
inline virtual std::optional<Obstacle> item() const
Get the (optional) obstacle attached to the robot’s TCP.
- Returns:
An (optional) obstacle attached to the robot’s TCP.
-
virtual void set_item(const std::optional<Obstacle> &item)
Set the (optional) obstacle attached to the robot’s TCP.
- Parameters:
item – An (optional) obstacle attached to the robot’s TCP.
-
virtual void set_speed(double speed) override
Sets the velocity, acceleration, and jerk limits to a factor [0, 1] of their respective default (maximum) values.
- Parameters:
speed – A double representing the speed to be set for the robot.
-
virtual void set_base(const Frame &base, const Frame &parent = Frame::Identity()) override
Sets the origin (base frame) of the robot.
- Parameters:
base – The
Frame
representing the new base or origin of the robot.parent – An optional
Frame
representing the parent frame. Defaults to the identity frame.
-
inline Frame flange_to_tcp() const
The transformation from the robot’s flange to the robot’s TCP.
- Returns:
A
Frame
representing the transformation from the robot’s flange to the robot’s TCP.
-
void set_flange_to_tcp(const Frame &flange_to_tcp)
Sets the transformation from the robot’s flange to the robot’s TCP.
- Parameters:
flange_to_tcp – A
Frame
representing the transformation from the robot’s flange to the robot’s TCP.
-
inline Frame world_base() const
Retrieves the frame of the robot’s base.
- Returns:
A
Frame
representing the frame of the robot’s base.
-
inline Frame flange() const
Retrieves the frame of the robot’s flange.
- Returns:
A
Frame
representing the frame of the robot’s flange.
-
inline Frame tcp() const
Retrieves the frame of the robot’s TCP.
- Returns:
A
Frame
representing the frame of the robot’s TCP.
-
inline virtual Frame tcp_position() const
Retrieves the frame of the robot’s TCP.
- Returns:
A
Frame
representing the frame of the robot’s TCP.
-
inline virtual Twist tcp_velocity() const
Retrieves the Cartesian velocity of the robot’s TCP.
- Returns:
A
Twist
representing the Cartesian velocity of the robot’s TCP.
-
inline virtual Twist tcp_acceleration() const
Retrieves the Cartesian acceleration of the robot’s TCP.
- Returns:
A
Twist
representing the Cartesian acceleration of the robot’s TCP.
-
Frame calculate_tcp(const Config &joint_position)
Calculates the forward_kinematics and returns the frame of the robot’s TCP.
- Parameters:
joint_position – The joint position of the robot.
- Returns:
The frame of the robot’s TCP.
-
double calculate_tcp_speed(const Config &joint_position, const Config &joint_velocity)
Calculates the forward_kinematics and returns the norm of the Cartesian velocity of the robot’s TCP.
- Parameters:
joint_position – The joint position of the robot.
joint_velocity – The joint velocity of the robot.
- Returns:
The Cartesian speed of the TCP.
-
virtual Jacobian calculate_jacobian() const = 0
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
Public Members
-
const size_t degrees_of_freedom
The degrees of freedom (or number of axis) of the robot.
-
const size_t number_joints
The number of joints with links in between.
-
const Config default_position
The default robot position - used for initializing the current robot position.
-
Config position
The current (or last) position of the robot used for planning. Mostly relevant for multi-robot planning.
-
std::optional<double> control_rate
The (optional) default control rate. [Hz].
-
std::vector<Obstacle> link_obstacles
The obstacles for each robot link.
-
Config min_position
Minimum position for each joint. [rad].
-
Config max_position
Maximum position for each joint. [rad].
-
Config max_velocity
Maximum absolute velocity for each joint. [rad/s].
-
Config max_acceleration
Maximum absolute acceleration for each joint. [rad/s^2].
-
Config max_jerk
Maximum absolute jerk for each joint. [rad/s^3].
-
enum class InverseKinematicsMethod
-
class DualArm : public jacobi::Robot
Subclassed by jacobi::robots::ABBYuMiIRB14000
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual std::optional<double> get_control_rate() const override
Retrieves the control rate of the robot.
- Returns:
An optional double representing the control rate (in Hz) of the robot. If not available, the method will return std::nullopt.
-
virtual size_t get_degrees_of_freedom() const override
Retrieves the degrees of freedom of the robot.
- Returns:
The degrees of freedom (DOFs) or number of axes of the robot.
-
virtual Config get_position() const override
Retrieves the current position of the robot.
- Returns:
A
Config
object representing the current joint positions of the robot.
-
virtual Config get_min_position() const override
Retrieves the minimum position limits of the robot.
- Returns:
A
Config
object representing the minimum joint position limits of the robot.
-
virtual Config get_max_position() const override
Retrieves the maximum position limits of the robot.
- Returns:
A
Config
object representing the maximum joint position limits of the robot.
-
virtual Config get_max_velocity() const override
Retrieves the maximum velocity limits of the robot.
- Returns:
A
Config
object representing the maximum joint velocity limits of the robot.
-
virtual Config get_max_acceleration() const override
Retrieves the maximum acceleration limits of the robot.
- Returns:
A
Config
object representing the maximum joint acceleration limits of the robot.
-
virtual Config get_max_jerk() const override
Retrieves the maximum jerk limits of the robot.
- Returns:
A
Config
object representing the maximum joint jerk limits of the robot.
-
virtual void set_base(const Frame &base, const Frame &parent = Frame::Identity()) override
Helper methods.
-
virtual void set_speed(double speed) override
Sets the velocity, acceleration, and jerk limits to a factor [0, 1] of their respective default (maximum) values.
- Parameters:
speed – A double representing the speed to be set for the robot.
-
virtual void to_json(nlohmann::json &j) const override
Serialization.
-
virtual std::shared_ptr<Robot> clone() const override
-
class ABBGoFaCRB1500010 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class ABBIRB1200590 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class ABBIRB1200770 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class ABBIRB1300714 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class ABBIRB1600612 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class ABBIRB260012185 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class ABBIRB460060205 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class ABBIRB6700150320 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class ABBYuMiIRB14000 : public jacobi::robots::DualArm
-
class Arm : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class Arm : public jacobi::RobotArm
-
class FanucCRX30iA : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class FanucLR10iA10 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class FanucLRMate200iD7L : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class FanucM20iB25 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class FanucM20iD25 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class FanucM710iC45M : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class FlexivRizon4 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class FlexivRizon4S : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class FlexivRizon10 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class FlexivRizon10S : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class FrankaPanda : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class KinovaGen37DoF : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class KukaIiwa7 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class KukaKR6R700sixx : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class KukaKR70R2100 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class MecademicMeca500 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class UfactoryXArm7 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class UniversalUR5e : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class UniversalUR10 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class UniversalUR10e : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class UniversalUR20 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class YaskawaGP12 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class YaskawaGP50 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class YaskawaGP180 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class YaskawaGP180120 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class YaskawaHC10 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
-
class YaskawaHC20 : public jacobi::RobotArm
Public Functions
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
virtual std::shared_ptr<Robot> clone() const override
Warning
Custom robots are in a beta preview with rudimentary support right now. If possible, use robots from our supported robot library, as they will be significantly faster to compute and easier to use.
-
class CustomRobot : public jacobi::RobotArm
A custom robot arm that can be loaded from a URDF file.
The
CustomRobot
class extends theRobotArm
class and provides the functionality to load a robot’s configuration from a URDF (Unified Robot Description Format) file. It also includes methods for handling inverse kinematics and filtering relevant configurations.Public Types
-
enum class JointType
Types of joints that can be present in the robot.
Values:
-
enumerator Revolute
A revolute joint that allows rotation.
-
enumerator Continuous
A continuous joint that allows unlimited rotation.
-
enumerator Prismatic
A prismatic joint that allows linear motion.
-
enumerator Fixed
A fixed joint that does not allow any motion.
-
enumerator Revolute
Public Functions
-
explicit CustomRobot(size_t degrees_of_freedom)
Constructs a CustomRobot with a specified degrees of freedom.
- Parameters:
degrees_of_freedom – The degrees of freedom for the robot.
-
explicit CustomRobot(size_t degrees_of_freedom, size_t number_joints)
Constructs a CustomRobot with a specified degrees of freedom and joints.
- Parameters:
degrees_of_freedom – The degrees of freedom for the robot.
number_joints – The number of joints in the robot.
-
virtual std::shared_ptr<Robot> clone() const override
Clones the current robot.
- Returns:
A shared pointer to a new instance of the robot that is a copy of the current one.
-
virtual Jacobian calculate_jacobian() const override
Calculates the Jacobian of the current robot’s kinematics.
- Returns:
The Jacobi matrix representing the derivative of robot’s TCP over the joint space
-
inline virtual size_t get_degrees_of_freedom() const override
Retrieves the degrees of freedom of the robot.
- Returns:
The degrees of freedom (DOFs) or number of axes of the robot.
-
inline virtual Config get_position() const override
Retrieves the current position of the robot.
- Returns:
A
Config
object representing the current joint positions of the robot.
-
inline virtual Config get_min_position() const override
Retrieves the minimum position limits of the robot.
- Returns:
A
Config
object representing the minimum joint position limits of the robot.
-
inline virtual Config get_max_position() const override
Retrieves the maximum position limits of the robot.
- Returns:
A
Config
object representing the maximum joint position limits of the robot.
-
inline virtual Config get_max_velocity() const override
Retrieves the maximum velocity limits of the robot.
- Returns:
A
Config
object representing the maximum joint velocity limits of the robot.
-
inline virtual Config get_max_acceleration() const override
Retrieves the maximum acceleration limits of the robot.
- Returns:
A
Config
object representing the maximum joint acceleration limits of the robot.
-
inline virtual Config get_max_jerk() const override
Retrieves the maximum jerk limits of the robot.
- Returns:
A
Config
object representing the maximum joint jerk limits of the robot.
-
inline virtual Frame tcp_position() const override
Retrieves the frame of the robot’s TCP.
- Returns:
A
Frame
representing the frame of the robot’s TCP.
-
inline virtual Twist tcp_velocity() const override
Retrieves the Cartesian velocity of the robot’s TCP.
- Returns:
A
Twist
representing the Cartesian velocity of the robot’s TCP.
-
inline virtual Twist tcp_acceleration() const override
Retrieves the Cartesian acceleration of the robot’s TCP.
- Returns:
A
Twist
representing the Cartesian acceleration of the robot’s TCP.
-
inline virtual std::optional<Obstacle> end_effector() const override
Get the (optional) end effector attached to the robot’s flange.
- Returns:
An (optional) end effector attached to the robot’s flange.
-
virtual void set_end_effector(const std::optional<Obstacle> &end_effector) override
Set the (optional) end effector attached to the robot’s flange.
- Parameters:
item – An (optional) end effector attached to the robot’s flange.
-
inline virtual std::optional<Obstacle> item() const override
Get the (optional) obstacle attached to the robot’s TCP.
- Returns:
An (optional) obstacle attached to the robot’s TCP.
-
virtual void set_item(const std::optional<Obstacle> &item) override
Set the (optional) obstacle attached to the robot’s TCP.
- Parameters:
item – An (optional) obstacle attached to the robot’s TCP.
-
std::optional<Config> inverse_kinematics(const CartesianWaypoint &waypoint)
Computes the inverse kinematics for a Cartesian waypoint.
- Parameters:
waypoint – The Cartesian waypoint to compute the inverse kinematics for.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const Config &reference_config)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
-
std::optional<Config> inverse_kinematics(const Frame &tcp, const std::optional<Config> &reference_config = std::nullopt)
Computes the inverse kinematics for a Cartesian position and a reference configuration.
Finds a joint position so that the robot’s TCP is at the given frame, which is defined in the world coordinate system. In general, the solution will try to stay close to the reference_config parameter. We use a numerical optimization for robots with more than 6 degrees of freedom. Then, the reference configuration is used as a starting point for the optimization. This method does not take the environment’s collision model into account.
- Parameters:
tcp – The Cartesian position to compute the inverse kinematics for.
reference_config – The reference configuration to use for the inverse kinematics.
- Returns:
An optional
Config
object representing the joint positions of the robot.
Public Members
-
std::shared_ptr<RobotArm> child
Possible child robot.
-
std::vector<Frame> link_translations
The translations between the links.
-
std::vector<std::array<double, 3>> joint_axes
Axes of the joints in the robot.
-
std::vector<JointType> joint_types
The type of the joints: Currently revolute, continuous, prismatic, and fixed joints are supported.
-
std::vector<std::string> joint_names
Names of the joints in the robot.
-
std::vector<std::string> config_joint_names
Names of the joints corresponding to a specific joint configuration.
Public Static Functions
-
static std::shared_ptr<CustomRobot> load_from_urdf_file(const std::filesystem::path &file, const std::string &base_link = "base_link", const std::string &end_link = "flange")
Load the robot from a URDF file.
Loads a custom robot from a *.urdf file, and sets the robot arm to the kinematic chain between the given base_link and the end_link.
- Parameters:
file – The path to the URDF file.
base_link – The name of the base link in the URDF.
end_link – The name of the end link in the URDF.
- Returns:
A shared pointer to the loaded robot.
-
enum class JointType
Motions¶
-
struct LinearSection
Represents a linear Cartesian section for either the approach to the goal or the retraction from the start.
Public Types
-
enum class Approximation
Whether to approximate the Cartesian linear motion in joint space for singularity-free calculation.
Values:
-
enumerator Never
-
enumerator Always
-
enumerator Never
Public Members
-
Frame offset
Relative linear cartesian offset from the reference pose.
-
double speed = {1.0}
Speed of the sub-motion, relative to the overall motion’s speed.
-
bool smooth_transition = {true}
Whether to use a smooth transition between this and the next or previous section. If false, the robot will come to a complete stop at the transition point.
-
enum class Approximation
-
class Motion
Represents a request for a collision-free point-to-point motion.
The Motion class provides an interface for general point-to-point motion planning with arbitrary waypoints, linear approach and retraction, and task constraints. It includes parameters for the motion name, robot, start and goal points, and additional settings for motion planning, such as collision checking and soft failure handling.
Public Functions
-
inline explicit Motion(const Point &start, const Point &goal)
Construct a Motion with a given start and goal point.
- Parameters:
start – The start point of the motion.
goal – The goal point of the motion.
Construct a Motion with a name, start and goal point.
- Parameters:
name – The unique name of the motion.
start – The start point of the motion.
goal – The goal point of the motion.
-
inline explicit Motion(const std::string &name, const Point &start, const Point &goal)
Construct a Motion with a name, start and goal point.
- Parameters:
name – The unique name of the motion.
start – The start point of the motion.
goal – The goal point of the motion.
Construct a Motion with a name, robot, start and goal point.
- Parameters:
name – The unique name of the motion.
robot – The robot for the motion.
start – The start point of the motion.
goal – The goal point of the motion.
-
inline explicit Motion()
Default constructor.
-
inline std::shared_ptr<RobotArm> robot_arm() const
The robot arm associated with the motion.
Check if the motion start and goal points are within the provided start and goal points.
- Parameters:
start_test – The start point to compare against.
goal_test – The goal point to compare against.
- Returns:
true If the motion start and goal points are within the provided start and goal points.
- Returns:
false If the motion start and goal points are not within the provided start and goal points.
Public Members
-
std::string name
The unique name of the motion.
-
Point start
Start point of the motion.
-
Point goal
Goal point of the motion.
-
std::shared_ptr<Robot> robot
The robot for the motion (e.g. defines the kinematics and the joint limits).
-
bool ignore_collisions = {false}
Whether to ignore collisions.
-
std::vector<ExactPoint> waypoints
Intermediate waypoints that the motion passes through exactly. The list of waypoints is limited to less than four, otherwise please take a look at LowLevelMotion.
-
std::optional<LinearSection> linear_retraction
Optional relative linear cartesian motion for retracting from the start pose.
-
std::optional<LinearSection> linear_approach
Optional relative linear cartesian motion for approaching the goal pose.
-
double path_length_loss_weight = {0.1}
Weight of the loss minimizing the path length of the trajectory.
-
double orientation_loss_weight = {0.0}
Weight of the loss minimizing the maximizing deviation of the end-effector orientation to the target value.
-
std::array<double, 3> orientation_target = {{0.0, 0.0, 1.0}}
Target vector pointing in the direction of the end-effector (TCP) orientation in the global coordinate system.
-
std::optional<double> cartesian_tcp_speed_cutoff
Optional Cartesian TCP speed (translation-only) cutoff. This is a post-processing step.
-
std::optional<std::vector<ExactPoint>> initial_waypoints
Optional initial waypoints to start the optimization with (don’t use with intermediate waypoints).
-
inline explicit Motion(const Point &start, const Point &goal)
-
class LinearMotion
Represents a request for a linear Cartesian-space motion.
The LinearMotion struct represents a request for a linear motion in Cartesian space. It consists of a start and goal point, as well as a robot to perform the motion. It provides an interface for planning singularity-free linear motion in Cartesian space between any two waypoints.
Public Functions
-
inline explicit LinearMotion(const ExactPoint &start, const ExactPoint &goal)
Construct a linear motion with a given start, and goal.
- Parameters:
start – The start point of the motion.
goal – The goal point of the motion.
Construct a linear motion with a given robot, start, and goal.
- Parameters:
robot – The robot for the motion.
start – The start point of the motion.
goal – The goal point of the motion.
- inline explicit LinearMotion(const std
-
inline explicit LinearMotion(const ExactPoint &start, const ExactPoint &goal)