def cost_position(x_des, fk):
return norm(fk[:3] - x_des[:3]) ** 2
微信
公众号
扫码关注公众号
微信
公众号