def solve_inverse_kinematics(mbp, target_frame, target_pose,
max_position_error=0.005, theta_bound=0.01*np.pi, initial_guess=None):
if initial_guess is None:
# TODO: provide initial guess for some joints (like iiwa joint0)
initial_guess = np.zeros(mbp.num_positions())
for joint in prune_fixed_joints(get_joints(mbp)):
lower, upper = get_joint_limits(joint)
if -np.inf < lower < upper < np.inf:
initial_guess[joint.position_start()] = random.uniform(lower, upper)
assert mbp.num_positions() == len(initial_guess)
ik_scene = InverseKinematics(mbp)
world_frame = mbp.world_frame()
ik_scene.AddOrientationConstraint(
frameAbar=target_frame, R_AbarA=RotationMatrix.Identity(),
frameBbar=world_frame, R_BbarB=RotationMatrix(target_pose.rotation()),
theta_bound=theta_bound)
lower = target_pose.translation() - max_position_error
upper = target_pose.translation() + max_position_error
ik_scene.AddPositionConstraint(
frameB=target_frame, p_BQ=np.zeros(3),
frameA=world_frame, p_AQ_lower=lower, p_AQ_upper=upper)
# AddAngleBetweenVectorsConstraint
# AddGazeTargetConstraint
prog = ik_scene.prog()
prog.SetInitialGuess(ik_scene.q(), initial_guess)
result = prog.Solve()
if result != SolutionResult.kSolutionFound:
return None
return prog.GetSolution(ik_scene.q())
def get_ik(self, t):
epsilon = 1e-2
ik = InverseKinematics(plant=self.plant, with_joint_limits=True)
if self.q_guess is None:
context = self.plant.CreateDefaultContext()
self.q_guess = self.plant.GetPositions(context)
# This helps get the solver out of the saddle point when knee joint are locked (0.0)
self.q_guess[getJointIndexInGeneralizedPositions(
self.plant, 'l_leg_kny')] = 0.1
self.q_guess[getJointIndexInGeneralizedPositions(
self.plant, 'r_leg_kny')] = 0.1
for trajectory in self.trajectories:
position = trajectory.position_traj.value(t)
ik.AddPositionConstraint(
frameB=self.plant.GetFrameByName(trajectory.frame),
p_BQ=trajectory.position_offset,
frameA=self.plant.world_frame(),
p_AQ_upper=position + trajectory.position_tolerance,
p_AQ_lower=position - trajectory.position_tolerance)
if trajectory.orientation_traj is not None:
orientation = trajectory.orientation_traj.value(t)
ik.AddOrientationConstraint(
frameAbar=self.plant.world_frame(),
R_AbarA=RotationMatrix.Identity(),
frameBbar=self.plant.GetFrameByName(trajectory.frame),
R_BbarB=RotationMatrix(orientation),
theta_bound=trajectory.orientation_tolerance)
result = Solve(ik.prog(), self.q_guess)
if result.is_success():
return result.GetSolution(ik.q())
else:
raise Exception("Failed to find IK solution!")
