Joint-space Half-spaces constraint#
Sometime, the workspace of a robot can be bound by a simple box, using joint limits.
However, for more complex robots where self-collisions are present, it can be considered to use a more complex set of inequalities, bounding the robot joint-space in a polytope of the form:
Where \(A\) is a \(n_c \times n_q\) matrix, and \(b\) a \(n_c\) vector. \(n_c\) being the number of constraints and \(n_q\) the dimension of the robot configuration space (state.q).
Each row of such a constraint is actually an half-space in the robot configuration space. This is what joint-space half-spaces constraint does.
Creating the constraint#
# Initializing A and b for one constraint
nq = len(robot.state.q)
A = np.zeros((1, nq))
b = np.zeros(1)
# We want r1 + r2 <= 1
A[0, robot.get_joint_offset("r1")] = 1
A[0, robot.get_joint_offset("r2")] = 1
b[0] = 1
# Adding the constraint
hspace_hspaces_constraint = solver.add_joint_space_half_spaces_constraint(A, b)
hspace_hspaces_constraint.configure("workspace", "hard")
You first need to create the matrix \(A\) and the vector \(b\) of proper sizes. Creating the constraint is then straightforward.
Note
Even if \(A\) has the same number of columns as the robot \(q\) vector for convenience, the floating base terms are ignored by JointSpaceHalfSpacesConstraint
Updating the constraint#
The polytope can be updated by setting A and b
# Updating the polytope
hspace_hspaces_constraint.A = new_A
hspace_hspaces_constraint.b = new_b
Example#
The JointSpaceHalfSpacesConstraint is used with a Polytope previously obtained with workspace estimation
SPM workspace limitation