Calculation¶
The Robot requires a certain model to calculate motion in 3D space. The following endpoints allow you to access this model to do calculation on your own.
Different concepts are used by these endpoints: joint angles, end effector position and orientation, waypoints and setpoints.
Concepts¶
Motion Errors¶
Motion Errors are used to represent errors that happened when calculating Robot's movement.
Code | Meaning |
---|---|
unknown |
Unknown error |
shoulder_singularity |
Shoulder singularity error |
shoulder_singularity_warning |
Shoulder singularity warning |
elbow_singularity |
Elbow singularity error |
elbow_singularity_warning |
Elbow singularity warning |
wrist_singularity |
Wrist singularity error |
wrist_singularity_warning |
Wrist singularity warning |
singularity |
Other singularity error |
ik_failure |
Inverse Kinematics failure |
analytical_ik_failure |
Analytical Inverse Kinematics failure |
j0_boundary_limit |
Joint 1 boundary limit |
j1_boundary_limit |
Joint 2 boundary limit |
j2_boundary_limit |
Joint 3 boundary limit |
j3_boundary_limit |
Joint 4 boundary limit |
j4_boundary_limit |
Joint 5 boundary limit |
j5_boundary_limit |
Joint 6 boundary limit |
reach_limit |
Reach limit |
velocity_calculation_error |
Velocity calculation error |
acceleration_calculation_error |
Acceleration calculation error |
linear_path_calculation_error |
Linear path calculation error |
cubic_path_calculation_error |
Cubic path calculation error |
j0_angular_velocity_limit |
Joint 1 angle velocity limit |
j1_angular_velocity_limit |
Joint 2 angle velocity limit |
j2_angular_velocity_limit |
Joint 3 angle velocity limit |
j3_angular_velocity_limit |
Joint 4 angle velocity limit |
j4_angular_velocity_limit |
Joint 5 angle velocity limit |
j5_angular_velocity_limit |
Joint 6 angle velocity limit |
j0_angular_acceleration_limit |
Joint 1 angle acceleration limit |
j1_angular_acceleration_limit |
Joint 2 angle acceleration limit |
j2_angular_acceleration_limit |
Joint 3 angle acceleration limit |
j3_angular_acceleration_limit |
Joint 4 angle acceleration limit |
j4_angular_acceleration_limit |
Joint 5 angle acceleration limit |
j5_angular_acceleration_limit |
Joint 6 angle acceleration limit |
ee_over_speed |
End-effector over speed |
ee_over_acceleration |
End-effector over acceleration |
ee_over_energy |
End-effector over energy |
j0_over_energy |
Joint 1 over energy |
j1_over_energy |
Joint 2 over energy |
j2_over_energy |
Joint 3 over energy |
j3_over_energy |
Joint 4 over energy |
j4_over_energy |
Joint 5 over energy |
j5_over_energy |
Joint 6 over energy |
plane_collision |
Plane collision |
self_collision |
Self collision |
object_collision |
Software Avoidance Zone collision |
generation_error |
Segment generation error |
result_generation_error |
Segment result generation error |
time_too_short |
Requested time is too short and exceeds velocity limits |
time_too_short_switched_to_min_time |
Requested time is too short and exceeds velocity limits, switched to minimum time |
too_few_waypoints |
Too few waypoints in the segment |
too_many_waypoints |
Too many waypoints in the segment |
could_not_calculate_min_time |
Could not calculate minimum time |
Pose validity¶
This endpoint allows you to check if a set of joint angles is valid and would not result with the Robot colliding with itself or its workspace.
Request
PUT /api/v1/calc/pose_valid
Payload
{
"joints": JOINT_ANGLES
}
With optional tcp_config
and avoidance_zones
{
"joints": JOINT_ANGLES,
"tcp_config": TCP_CONFIG,
"avoidance_zones": AVOIDANCES_ZONES
}
where JOINT_ANGLES
is an array containing the joint angles of the Robot expressed in radians, e.g. [0,0,0,1.7,0.5,-1]
.
The tcp_config
property can also be specified in order to configure the TCP of the tool attached to the robot end-effector (see TCP configuration). The avoidance_zones
property can also be specified in order to configure zones where the robot won't go (see Software Avoidance Zones).
Reply
{
"pose": {
"valid": VALID_RESULT,
"status": VALID_RESULT_STRING
}
}
or an error.
Where every VALID_RESULT
is a boolean representing if the angles were valid or not, and VALID_RESULT_STRING
is a human-readable string explaining if the pose is valid.
Forward Kinematics¶
This endpoint allows you to calculate the end effector position and orientation using the joint angles of the Robot.
Note that when using the avoidance_zones
argument, this operation can fail, the result
property will be "success"
if all calculations went alright and "error"
otherwise. In the latter case, another property, error
, will be given indicating which motion error has happened.
Request
PUT /api/v1/calc/forward_kinematics
Payload
{
"joints": JOINT_ANGLES
}
With optional tcp_config
and avoidance_zones
{
"joints": JOINT_ANGLES,
"tcp_config": TCP_CONFIG,
"avoidance_zones": AVOIDANCES_ZONES
}
where JOINT_ANGLES
is an array containing the joint angles of the Robot expressed in radians, e.g. [0,0,0,1.7,0.5,-1]
.
The tcp_config
property can also be specified in order to configure the TCP of the tool attached to the robot end-effector (see TCP configuration). The avoidance_zones
property can also be specified in order to configure zones where the robot won't go (see Software Avoidance Zones).
Reply
- It works
{
"fk": {
"result": "success",
"position": {"x": POSITION_X, "y": POSITION_Y, "z": POSITION_Z},
"orientation": {"w": QUATERNION_W, "x": QUATERNION_X, "y": QUATERNION_Y, "z": QUATERNION_Z}
}
}
- It fails
{
"fk": {
"result": "error",
"error": MOTION_ERROR
}
}
or an error.
Where every *_X
, *_Y
and *_Z
is a float. The position
object is a vector in the end effector space (relative to the base of the Robot). Note that the position is a vector expressed in meters. The orientation is a quaternion that represents the end effector orientation from the base.
Inverse Kinematics¶
This endpoint allows you to calculate the joint angles of the Robot at a given end effector position. For the calculation to be correct, the service also needs the desired end effector orientation (expressed using 3 vectors) and a guess of the resulting joint angles.
Note that this operation can fail, the result
property will be "success"
if all calculations went alright and "error"
otherwise. In the latter case, another property, error
, will be given indicating which motion error has happened.
Request
PUT /api/v1/calc/inverse_kinematics
Payload
{
"guess": JOINT_ANGLES,
"position": EE_POSITION,
"orientation": EE_ORIENTATION
}
With optional tcp_config
and avoidance_zones
{
"guess": JOINT_ANGLES,
"position": EE_POSITION,
"orientation": EE_ORIENTATION,
"tcp_config": TCP_CONFIG,
"avoidance_zones": AVOIDANCES_ZONES
}
where JOINT_ANGLES
is an array containing the joint angles of the Robot expressed in radians, e.g. [0, 0, 0, 1.7, 0.5, -1]
; EE_POSITION
is an object containing 3 properties: x
, y
and z
, each being a float, thus representing a 3D vector (expressed in meters). EE_ORIENTATION
is an object that represents the quaternion of the orientation. It contains 4 properties: w
, x
, y
and z
, each being a float.
The tcp_config
property can also be specified in order to configure the TCP of the tool attached to the robot end-effector (see TCP configuration). The avoidance_zones
property can also be specified in order to configure zones where the robot won't go (see Software Avoidance Zones).
Example replies
- It works
{
"ik": {
"result": "success",
"joints": [-2.3, 0, 0, 0.32, 0, 0.78]
}
}
- It fails
{
"ik": {
"result": "error",
"error": MOTION_ERROR
}
}
or an error.
Where MOTION_ERROR
is a motion error code.
Nudge¶
This endpoint allows you to calculate the joint angles of the Robot after a "nudge" (moving of a given offset in a given direction). This transformation maintains end effector orientation.
Note that this operation can fail, the result
property will be "success"
if all calculations went alright and "error"
otherwise. In the latter case, another property, error
, will be given indicating which motion error has happened.
Request
PUT /api/v1/calc/nudge
Payload
{
"joints": JOINT_ANGLES,
"direction": NUDGE_DIRECTION,
"offset": NUDGE_OFFSET
}
With optional tcp_config
and avoidance_zones
{
"joints": JOINT_ANGLES,
"direction": NUDGE_DIRECTION,
"offset": NUDGE_OFFSET,
"tcp_config": TCP_CONFIG,
"avoidance_zones": AVOIDANCES_ZONES
}
where JOINT_ANGLES
is an array containing the joint angles of the Robot expressed in radians, e.g. [0, 0, 0, 1.7, 0.5, -1]
; NUDGE_DIRECTION
is a string which can be either "x"
, "y"
or "z"
; NUDGE_OFFSET
is a float representing the distance in meters, e.g. 0.2
.
The tcp_config
property can also be specified in order to configure the TCP of the tool attached to the robot end-effector (see TCP configuration). The avoidance_zones
property can also be specified in order to configure zones where the robot won't go (see Software Avoidance Zones).
Example replies
- It works
{
"nudge": {
"result": "success",
"joints": [-2.3, 0, 0, 0.32, 0, 0.78]
}
}
- It fails
{
"nudge": {
"result": "error",
"error": MOTION_ERROR
}
}
or an error.
Where MOTION_ERROR
is a motion error code.
Rotate¶
This endpoint allows you to calculate the joint angles of the Robot after a rotation of the end effector of a given offset in a given direction. This transformation maintains end effector position.
Note that this operation can fail, the result
property will be "success"
if all calculations went alright and "error"
otherwise. In the latter case, another property, error
, will be given indicating which motion error has happened.
Request
PUT /api/v1/calc/rotate
Payload
{
"joints": JOINT_ANGLES,
"axis": ROTATE_AXIS,
"offset": ROTATE_OFFSET
}
With optional tcp_config
and avoidance_zones
{
"joints": JOINT_ANGLES,
"axis": ROTATE_AXIS,
"offset": ROTATE_OFFSET,
"tcp_config": TCP_CONFIG,
"avoidance_zones": AVOIDANCES_ZONES
}
where JOINT_ANGLES
is an array containing the joint angles of the Robot expressed in radians, e.g. [0, 0, 0, 1.7, 0.5, -1]
; ROTATE_AXIS
is a string which can be either "x"
, "y"
or "z"
; ROTATE_OFFSET
is a float representing the angle in radians, e.g. 0.5
.
The tcp_config
property can also be specified in order to configure the TCP of the tool attached to the robot end-effector (see TCP configuration). The avoidance_zones
property can also be specified in order to configure zones where the robot won't go (see Software Avoidance Zones).
Example replies
- It works
{
"rotate": {
"result": "success",
"joints": [-2.3, 0, 0, 0.32, 0, 0.78]
}
}
- It fails
{
"rotate": {
"result": "error",
"error": MOTION_ERROR
}
}
or an error.
Where MOTION_ERROR
is a motion error code.