I wanted to make a mini-post with a sort of cute problem I solved today: let’s say you want your robotic arm to travel from one joint-space position to another, but along the way you want it to follow a Cartesian end-effector trajectory. This actually isn’t such an unrealistic scenario! You’re basically constraining the start and end to be “sort of reasonable” (something you might want to do, given that 7+dof arms can have weird/slow ik), but you only care about the end effector path in between them, not the whole joint trajectory. My solution to this is to iteratively:
- Find the inverse Jacobian at the current state with respect to end effector linear velocity
- Use this to find a JS vector that matches a desired cartesian velocity: ($v_{j} = J^{-1} v_{c}$)
- Solve/find the null space of the Jacobian $N(J)$
- Project a vector towards the joint goal from the current state into the null space: $\texttt{proj}_{N(J)} v^*_j$
- Add the null space vector to the existing JS velocity vector from step 2 (this will change orientation/move towards the JS goal with no change to the linear velocity): $\hat{v} = v_{j} + \texttt{proj}_{N(J)} v^*_j$
- Step in the resulting direction by some small delta and starting this process over
The resuting solution is very fast, even in Python, and seems pretty reliable! The fidelity to your desired trajectory mostly depends on the sampling, but something like this is fine for me:
import numpy as np
import kinpy as kp
import scipy
DT = 0.01
PKG_DIR = '.'
jaco_ee_link = "EndEffector_Link"
urdf_path = os.path.join(PKG_DIR, 'gen3.urdf')
gen3_chain = kp.build_serial_chain_from_urdf(open(urdf_path).read(), jaco_ee_link)
def jaco_ee_fk(posn):
ee_trans = gen3_chain.forward_kinematics(posn)
return np.asarray(ee_trans.pos)
def with_max_mag(vec, mag):
vec_norm = np.dot(vec, vec)
if np.sqrt(vec_norm) < mag:
return vec
return (mag/vec_norm)*vec
def js_sub(vec1, vec2):
diff = vec1 - vec2
diff[diff > np.pi] -= 2*np.pi
diff[diff < -np.pi] += 2*np.pi
return diff
def cart2jnt(cart_traj, joint_start, joint_goal):
"""
cart_traj: (ts, 3) a cartesian-space ee trajectory to follow
joint_start: (7) starting joint-space position (must have
ee_fk(joint_start) == cart_traj[0,:])
joint_goal: (7) ending joint-space position (must have
ee(fk(joint_goal) == cart_traj[-1,:]))
Returns: (ts, 7) joint-space trajectory from joint_start to joint_goal
that follows cart_traj with the end effector
"""
fake_goal = joint_start + js_sub(joint_goal, joint_start)
traj = cart2jnt_hlpr(cart_traj, joint_start, fake_goal)
if len(traj) == 0:
return np.array([fake_goal])
elif not np.all(traj[-1] == fake_goal):
traj.append(fake_goal)
return np.array(traj)
def cart2jnt_hlpr(cart_traj, joint_start, joint_goal):
"""
INTERNAL
cart_traj: (ts, 3) a cartesian-space ee trajectory to follow
joint_start: (7) starting joint-space position (must have
ee_fk(joint_start) == cart_traj[0,:])
joint_goal: (7) ending joint-space position (must have
ee(fk(joint_goal) == cart_traj[-1,:]))
Returns: non-np Array of joint-space states from joint_start to
somewhere close to joint_goal that follows cart_traj with the
end effector
"""
curr_state = joint_start.copy()
traj = []
cart_vel = 0.15
jnt_vel = 0.25
next_traj_idx = 0
while True:
traj.append(curr_state)
# check for completed waypoints and get next state
curr_cart_posn = jaco_ee_fk(curr_state)
next_cart_pt = cart_traj[next_traj_idx,:]
while np.linalg.norm(curr_cart_posn - next_cart_pt) < 0.05:
next_traj_idx += 1
if next_traj_idx == cart_traj.shape[0]:
return traj
next_cart_pt = cart_traj[next_traj_idx,:]
cart_dir = with_max_mag(next_cart_pt - curr_cart_posn, cart_vel)
# find robot jacobian, first 3 are linear vel
J = gen3_chain.jacobian(curr_state)
# inverse jacobian will let us go from cart vel to joint vel
J_inv = np.linalg.pinv(J)
# multiply robot direction by J_inv to get translation component
oriented_cart_dir = np.zeros(6)
oriented_cart_dir[:3] = cart_dir
curr_vel = np.matmul(J_inv, oriented_cart_dir).ravel()
joint_dir = with_max_mag(joint_goal - curr_state, jnt_vel)
# find the null space of the Jacobian to see vectors that can get us
# close jointwise
J_null = scipy.linalg.null_space(J[:3, :])
# project JS-linear vector into the null space
J_null_inv = np.linalg.pinv(J_null)
null_weights = np.matmul(J_null_inv, joint_dir)
# get the null vector closest to the JS-linear vector and add to vel
null_proj = np.matmul(J_null, null_weights)
curr_vel += null_proj.ravel()
curr_cart_vel = np.linalg.norm(np.matmul(J, curr_vel).ravel()[:3])
if curr_cart_vel > cart_vel:
curr_vel = (cart_vel/curr_cart_vel)*curr_vel
curr_state = curr_state + DT*curr_vel
if __name__ == "__main__":
# two positions relevant to my work
jnt_start = np.array([ 2.09840329, 0.92247539, -2.3143059 , -0.65085367, -0.88642678, -1.33266019, -0.83731862])
jnt_goal = np.array([ 0.82179658, 0.73568667, -2.32717109, -0.96308693, -0.59917077, -1.53383378, -1.67858404])
start_cart = jaco_ee_fk(jnt_start)
goal_cart = jaco_ee_fk(jnt_goal)
cart_traj = np.linspace(start_cart, goal_cart, 6)
traj = cart2jnt(cart_traj, jnt_start, jnt_goal)
print("Desired cart traj: ")
print(repr(cart_traj))
print("="*20)
print("Actual cart traj: ")
print(repr(np.array([jaco_ee_fk(x) for x in traj])))
I hope to add language-specific syntax highlighting to this blog at some point so this is actually readable :(