def distance_from_goal(self, trajectory, goal):
reached_goal = self.fk.get(trajectory[-1])
return norm(reached_goal[0] - goal[0])
微信
公众号
扫码关注公众号