def setUp(self):
builder = DiagramBuilder()
self.plant, self.scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.01))
Parser(self.plant).AddModelFromFile(FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
self.plant.Finalize()
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(
self.plant, diagram_context)
self.body1_frame = self.plant.GetBodyByName("body1").body_frame()
self.body2_frame = self.plant.GetBodyByName("body2").body_frame()
self.ik_two_bodies = ik.InverseKinematics(
plant=self.plant, plant_context=plant_context)
# Test non-SceneGraph constructor.
ik.InverseKinematics(plant=self.plant)
self.prog = self.ik_two_bodies.get_mutable_prog()
self.q = self.ik_two_bodies.q()
# Test constructor without joint limits
ik.InverseKinematics(plant=self.plant, with_joint_limits=False)
ik.InverseKinematics(
plant=self.plant, plant_context=plant_context,
with_joint_limits=False)
def squaredNorm(x):
return np.array([x[0] ** 2 + x[1] ** 2 + x[2] ** 2 + x[3] ** 2])
self.prog.AddConstraint(
squaredNorm, [1], [1], self._body1_quat(self.q))
self.prog.AddConstraint(
squaredNorm, [1], [1], self._body2_quat(self.q))
self.prog.SetInitialGuess(self._body1_quat(self.q), [1, 0, 0, 0])
self.prog.SetInitialGuess(self._body2_quat(self.q), [1, 0, 0, 0])
def GetHomeConfiguration(is_printing=True):
"""
Returns a configuration of the MultibodyPlant in which point Q (defined by global variable p_EQ)
in robot EE frame is at p_WQ_home, and orientation of frame Ea is R_WEa_ref.
"""
# get "home" pose
ik_scene = inverse_kinematics.InverseKinematics(plant)
theta_bound = 0.005 * np.pi # 0.9 degrees
X_EEa = GetEndEffectorWorldAlignedFrame()
R_EEa = RotationMatrix(X_EEa.rotation())
ik_scene.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WL7_ref,
frameBbar=l7_frame,
R_BbarB=RotationMatrix.Identity(),
theta_bound=theta_bound)
p_WQ0 = p_WQ_home
p_WQ_lower = p_WQ0 - 0.005
p_WQ_upper = p_WQ0 + 0.005
ik_scene.AddPositionConstraint(frameB=l7_frame,
p_BQ=p_L7Q,
frameA=world_frame,
p_AQ_lower=p_WQ_lower,
p_AQ_upper=p_WQ_upper)
prog = ik_scene.prog()
prog.SetInitialGuess(ik_scene.q(), np.zeros(plant.num_positions()))
result = prog.Solve()
if is_printing:
print result
return prog.GetSolution(ik_scene.q())
def MoveEndEffectorAlongStraightLine(
p_WQ_start, p_WQ_end, duration, get_orientation, q_initial_guess, num_knots):
"""
p_WQ_start is the starting position of point Q (the point Q we defined earlier!)
p_WQ_end is the end position of point Q.
duration is the duration of the trajectory measured in seconds.
get_orientation(i, n) is a function that returns the interpolated orientation R_WL7 at the i-th knot point out of n knot points. It can simply return a constant orientation, but changing the orientation of link 7 should be helpful for placing the object on the shelf.
num_knots is the number of knot points in the trajectory. Generally speaking,the larger num_knots is, the smoother the trajectory, but solving for more knot points also takes longer. You can start by setting num_knots to 10, which should be sufficient for the task.
"""
t_knots = np.linspace(0, duration, num_knots+1)
q_knots = np.zeros((num_knots, plant.num_positions()))
q_knots[0] = q_initial_guess
n = len(q_initial_guess)
for i in range(num_knots):
ik = inverse_kinematics.InverseKinematics(plant)
prog = ik.prog()
p_AQ = p_WQ_start + (p_WQ_end - p_WQ_start) * i / (num_knots - 1)
ik.AddPositionConstraint(frameB=l7_frame, p_BQ=p_L7Q, frameA=world_frame, p_AQ_lower=p_AQ - 0.01, p_AQ_upper=p_AQ + 0.01) # interpolate between p_WQ_start and p_WQ_end
ik.AddOrientationConstraint(frameAbar=world_frame, R_AbarA=get_orientation(i, num_knots), frameBbar=l7_frame, R_BbarB=RotationMatrix(), theta_bound=theta_bound) # call get_orientation(i, num_knots).
if i == 0:
prog.SetInitialGuess(ik.q(), q_initial_guess)
else:
# This is very important for the smoothness of the whole trajectory.
prog.SetInitialGuess(ik.q(), q_knots[i - 1])
result = mp.Solve(ik.prog())
assert result.get_solution_result() == mp.SolutionResult.kSolutionFound
q_knots[i] = result.GetSolution(ik.q())
return PiecewisePolynomial.Cubic(t_knots[1:], q_knots.T, np.zeros(n), np.zeros(n)) # return a cubic spline that connects all q_knots.
def SolveOneShotIk(
p_WQ_ref,
R_WL7_ref,
p_L7Q,
q_initial_guess,
position_tolerance=0.005,
theta_bound=0.005):
ik = inverse_kinematics.InverseKinematics(plant)
ik.AddOrientationConstraint(
frameAbar=world_frame, R_AbarA=R_WL7_ref,
frameBbar=l7_frame, R_BbarB=RotationMatrix.Identity(),
theta_bound=theta_bound)
p_WQ_lower = p_WQ_ref - position_tolerance
p_WQ_upper = p_WQ_ref + position_tolerance
ik.AddPositionConstraint(
frameB=l7_frame, p_BQ=p_L7Q,
frameA=world_frame,
p_AQ_lower=p_WQ_lower, p_AQ_upper=p_WQ_upper)
prog = ik.prog()
prog.SetInitialGuess(ik.q(), q_initial_guess)
result = mp.Solve(prog)
if result.get_solution_result() != SolutionResult.kSolutionFound:
print(result.get_solution_result())
raise RuntimeError
return result.GetSolution(ik.q())
def GetHomeConfiguration(is_printing=True):
ik_scene = inverse_kinematics.InverseKinematics(plant)
theta_bound = 0.005 * np.pi
ik_scene.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WEa_ref,
frameBbar=gripper_frame,
R_BbarB=R_EEa,
theta_bound=theta_bound)
p_WQ0 = p_WQ_home
p_WQ_lower = p_WQ0 - 0.005
p_WQ_upper = p_WQ0 + 0.005
ik_scene.AddPositionConstraint(frameB=gripper_frame,
p_BQ=p_EQ,
frameA=world_frame,
p_AQ_lower=p_WQ_lower,
p_AQ_upper=p_WQ_upper)
prog = ik_scene.prog()
prog.SetInitialGuess(ik_scene.q(), np.zeros(plant.num_positions()))
result = prog.Solve()
if is_printing:
print(result)
return prog.GetSolution(ik_scene.q())
def GoFromPointToPoint(p_WQ_start,
p_WQ_end,
duration,
num_knot_points,
q_initial_guess,
InterpolatePosition=InterpolateStraightLine,
position_tolerance=0.005):
# The first knot point is the zero posture.
# The second knot is the pre-pre-grasp posture q_val_0
# The rest are solved for in the for loop below.
# The hope is that using more knot points makes the trajectory
# smoother.
q_knots = np.zeros((num_knot_points + 1, plant.num_positions()))
q_knots[0] = q_initial_guess
for i in range(num_knot_points):
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q()
# ik.AddOrientationConstraint(
# frameAbar=world_frame, R_AbarA=R_WEa_ref,
# frameBbar=gripper_frame, R_BbarB=R_EEa,
# theta_bound=theta_bound)
p_WQ = InterpolatePosition(p_WQ_start, p_WQ_end, num_knot_points,
i)
ik.AddPositionConstraint(frameB=gripper_frame,
p_BQ=p_EQ,
frameA=world_frame,
p_AQ_lower=p_WQ - position_tolerance,
p_AQ_upper=p_WQ + position_tolerance)
prog = ik.prog()
# use the robot posture at the previous knot point as
# an initial guess.
prog.SetInitialGuess(q_variables, q_knots[i])
result = prog.Solve()
if is_printing:
print i, ": ", result
q_knots[i + 1] = prog.GetSolution(q_variables)
t_knots = np.linspace(0, duration, num_knot_points + 1)
q_knots_kuka = GetKukaQKnots(q_knots)
qtraj = PiecewisePolynomial.Cubic(t_knots, q_knots_kuka.T, np.zeros(7),
np.zeros(7))
return qtraj, q_knots
def InverseKinPointwise(p_WQ_start,
p_WQ_end,
duration,
num_knot_points,
q_initial_guess,
InterpolatePosition=None,
position_tolerance=0.005,
theta_bound=0.005 * np.pi,
is_printing=True):
q_knots = np.zeros((num_knot_points + 1, plant.num_positions()))
q_knots[0] = q_initial_guess
for i in range(num_knot_points):
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q()
# Orientation constraint
ik.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WEa_ref,
frameBbar=gripper_frame,
R_BbarB=R_EEa,
theta_bound=theta_bound)
# Position constraint
p_WQ = InterpolatePosition(p_WQ_start, p_WQ_end, num_knot_points, i)
p_WQ_lower = p_WQ - position_tolerance
p_WQ_upper = p_WQ + position_tolerance
ik.AddPositionConstraint(frameB=gripper_frame,
p_BQ=p_EQ,
frameA=world_frame,
p_AQ_lower=p_WQ_lower,
p_AQ_upper=p_WQ_upper)
prog = ik.prog()
prog.SetInitialGuess(q_variables, q_knots[i])
result = prog.Solve()
if is_printing:
print(i, ": ", result)
q_knots[i + 1] = prog.GetSolution(q_variables)
t_knots = np.linspace(0, duration, num_knot_points + 1)
q_knots_kuka = GetKukaQKnots(q_knots)
qtraj = PiecewisePolynomial.Cubic(t_knots, q_knots_kuka.T, np.zeros(7),
np.zeros(7))
return qtraj, q_knots
def IK(self,
p,
q_initial_guess=None,
position_tolerance=0.005,
is_goal=False):
"""
:param p:
:param q_initial_guess: np array of shape (1, num positions for plant)
:param position_tolerance:
:return:
"""
if q_initial_guess is None:
q_initial_guess = np.random.rand(
1, self.plant.num_positions()) * 2 * np.pi - np.pi
plant = self.plant
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q()
# Orientation constraint
if is_goal:
theta_bound = 0.005 * np.pi
ik.AddOrientationConstraint(frameAbar=self.world_frame,
R_AbarA=R_WEa_ref,
frameBbar=self.gripper_frame,
R_BbarB=R_EEa,
theta_bound=theta_bound)
# Position constraint
p_lower = p - position_tolerance
p_upper = p + position_tolerance
ik.AddPositionConstraint(frameB=self.gripper_frame,
p_BQ=p_EQ,
frameA=self.world_frame,
p_AQ_lower=p_lower,
p_AQ_upper=p_upper)
prog = ik.prog()
prog.SetInitialGuess(q_variables, q_initial_guess)
prog.Solve()
q_plant = prog.GetSolution(q_variables)
return self.get_kuka_q(q_plant)
def setUp(self):
file_name = FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf")
self.plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(self.plant).AddModelFromFile(file_name)
self.plant.Finalize()
self.body1_frame = self.plant.GetBodyByName("body1").body_frame()
self.body2_frame = self.plant.GetBodyByName("body2").body_frame()
self.ik_two_bodies = ik.InverseKinematics(self.plant)
self.prog = self.ik_two_bodies.get_mutable_prog()
self.q = self.ik_two_bodies.q()
def squaredNorm(x):
return np.array([x[0]**2 + x[1]**2 + x[2]**2 + x[3]**2])
self.prog.AddConstraint(squaredNorm, [1], [1],
self._body1_quat(self.q))
self.prog.AddConstraint(squaredNorm, [1], [1],
self._body2_quat(self.q))
self.prog.SetInitialGuess(self._body1_quat(self.q), [1, 0, 0, 0])
self.prog.SetInitialGuess(self._body2_quat(self.q), [1, 0, 0, 0])
def GenerateIiwaPlansAndGripperSetPoints(manip_station_sim, is_printing=True):
plant = manip_station_sim.plant
tree = plant.tree()
iiwa_model = plant.GetModelInstanceByName("iiwa")
gripper_model = plant.GetModelInstanceByName("gripper")
# get "home" pose
ik_scene = inverse_kinematics.InverseKinematics(plant)
world_frame = plant.world_frame()
gripper_frame = plant.GetFrameByName("body", gripper_model)
theta_bound = 0.005 * np.pi # 0.9 degrees
X_EEa = GetEndEffectorWorldAlignedFrame()
R_EEa = RotationMatrix(X_EEa.rotation())
ik_scene.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WEa_ref,
frameBbar=gripper_frame,
R_BbarB=R_EEa,
theta_bound=theta_bound)
p_WQ0 = p_WQ_home
p_WQ_lower = p_WQ0 - 0.005
p_WQ_upper = p_WQ0 + 0.005
ik_scene.AddPositionConstraint(frameB=gripper_frame,
p_BQ=p_EQ,
frameA=world_frame,
p_AQ_lower=p_WQ_lower,
p_AQ_upper=p_WQ_upper)
prog = ik_scene.prog()
prog.SetInitialGuess(ik_scene.q(), np.zeros(plant.num_positions()))
result = prog.Solve()
if is_printing:
print result
q_val_0 = prog.GetSolution(ik_scene.q())
# q returned by IK consists of the configuration of all bodies, including
# the iiwa arm, the box, the gripper and the bottle.
# But the trajectory sent to iiwa only needs the configuration of iiwa.
# This function takes in an array of shape (n, plant.num_positions()),
# and returns an array of shape (n, 7), which only has the configuration of the iiwa arm.
def GetKukaQKnots(q_knots):
if len(q_knots.shape) == 1:
q_knots.resize(1, q_knots.size)
n = q_knots.shape[0]
q_knots_kuka = np.zeros((n, 7))
for i, q_knot in enumerate(q_knots):
q_knots_kuka[i] = tree.get_positions_from_array(iiwa_model, q_knot)
return q_knots_kuka
def InterpolateStraightLine(p_WQ_start, p_WQ_end, num_knot_points, i):
return (p_WQ_end - p_WQ_start) / num_knot_points * (i + 1) + p_WQ_start
# inverse_kin_ponitwise
def GoFromPointToPoint(p_WQ_start,
p_WQ_end,
duration,
num_knot_points,
q_initial_guess,
InterpolatePosition=InterpolateStraightLine,
position_tolerance=0.005):
# The first knot point is the zero posture.
# The second knot is the pre-pre-grasp posture q_val_0
# The rest are solved for in the for loop below.
# The hope is that using more knot points makes the trajectory
# smoother.
q_knots = np.zeros((num_knot_points + 1, plant.num_positions()))
q_knots[0] = q_initial_guess
for i in range(num_knot_points):
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q()
# ik.AddOrientationConstraint(
# frameAbar=world_frame, R_AbarA=R_WEa_ref,
# frameBbar=gripper_frame, R_BbarB=R_EEa,
# theta_bound=theta_bound)
p_WQ = InterpolatePosition(p_WQ_start, p_WQ_end, num_knot_points,
i)
ik.AddPositionConstraint(frameB=gripper_frame,
p_BQ=p_EQ,
frameA=world_frame,
p_AQ_lower=p_WQ - position_tolerance,
p_AQ_upper=p_WQ + position_tolerance)
prog = ik.prog()
# use the robot posture at the previous knot point as
# an initial guess.
prog.SetInitialGuess(q_variables, q_knots[i])
result = prog.Solve()
if is_printing:
print i, ": ", result
q_knots[i + 1] = prog.GetSolution(q_variables)
t_knots = np.linspace(0, duration, num_knot_points + 1)
q_knots_kuka = GetKukaQKnots(q_knots)
qtraj = PiecewisePolynomial.Cubic(t_knots, q_knots_kuka.T, np.zeros(7),
np.zeros(7))
return qtraj, q_knots
# Generating trajectories
num_knot_points = 10
# move to grasp left door handle
p_WQ_start = p_WQ0
p_WQ_end = p_WC_handle
qtraj_move_to_handle, q_knots_full = GoFromPointToPoint(
p_WQ_start,
p_WQ_end,
duration=5.0,
num_knot_points=num_knot_points,
q_initial_guess=q_val_0,
position_tolerance=0.001)
# close gripper
q_knots = np.zeros((2, 7))
q_knots[0] = qtraj_move_to_handle.value(
qtraj_move_to_handle.end_time()).squeeze()
qtraj_close_gripper = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)
# pull handle along an arc
def InterpolateArc(angle_start, angle_end, num_knot_points, i):
radius = r_handle
theta = angle_start + (angle_end - angle_start) * (i +
1) / num_knot_points
return p_WC_left_hinge + [
-radius * np.sin(theta), -radius * np.cos(theta), 0
]
angle_start = theta0_hinge
angle_end = np.pi / 180 * 60
qtraj_pull_handle, q_knots_full = GoFromPointToPoint(
angle_start,
angle_end,
duration=5.0,
num_knot_points=20,
q_initial_guess=q_knots_full[-1],
InterpolatePosition=InterpolateArc,
position_tolerance=0.002)
q_traj_list = [
qtraj_move_to_handle, qtraj_close_gripper, qtraj_pull_handle
]
plan_list = []
for q_traj in q_traj_list:
plan_list.append(JointSpacePlan(q_traj))
gripper_setpoint_list = [0.03, 0.0, 0.0]
return plan_list, gripper_setpoint_list
def pose_to_jointangles(T_world_robotPose):
plant, _ = CreateIiwaControllerPlant()
plant_context = plant.CreateDefaultContext()
world_frame = plant.world_frame()
gripper_frame = plant.GetFrameByName("body")
#q_nominal = np.array([ 0., 0.6, 0., -1.75, 0., 1., 0., 0., 0.]) # nominal joint for joint-centering.
q_nominal = np.array(
[-1.57, 0.1, 0.00, -1.2, 0.00, 1.60, 0.00, 0.00, 0.00])
def AddOrientationConstraint(ik, R_WG, bounds):
ik.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WG,
frameBbar=gripper_frame,
R_BbarB=RotationMatrix(),
theta_bound=bounds)
def AddPositionConstraint(ik, p_WG_lower, p_WG_upper):
ik.AddPositionConstraint(frameA=world_frame,
frameB=gripper_frame,
p_BQ=np.zeros(3),
p_AQ_lower=p_WG_lower,
p_AQ_upper=p_WG_upper)
# def AddJacobianConstraint_Joint_To_Plane(ik):
# # calculate the jacobian
# J_G = plant.CalcJacobianSpatialVelocity(
# ik.context(),
# JacobianWrtVariable.kQDot,
# gripper_frame,
# [0,0,0],
# world_frame,
# world_frame
# )
# # ensure that when joints 4 and 6 move, they keep the gripper in the desired plane
# prog = ik.get_mutable_prog()
# prog.AddConstraint()
# joint_4 = J_G[:, 3]
# joint_6 = J_G[:, 5]
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q() # Get variables for MathematicalProgram
prog = ik.prog() # Get MathematicalProgram
p_WG = T_world_robotPose.translation()
r_WG = T_world_robotPose.rotation()
# must be an exact solution
z_slack = 0
degrees_slack = 0
AddPositionConstraint(ik, p_WG - np.array([0, 0, z_slack]),
p_WG + np.array([0, 0, z_slack]))
AddOrientationConstraint(ik, r_WG, degrees_slack * np.pi / 180)
# todo: add some sort of constraint so that jacobian is 0 in certain directions
# (so just joints 4 and 6 move when swung)
# initial guess
prog.SetInitialGuess(q_variables, q_nominal)
diff = q_variables - q_nominal
prog.AddCost(np.sum(diff.dot(diff)))
result = Solve(prog)
if not result.is_success():
#visualize_transform(meshcat, "FAIL", X_WG, prefix='', length=0.3, radius=0.02)
assert (False) # no IK solution for target
jas = result.GetSolution(q_variables)
return jas
def create_q_knots(pose_lst):
"""Convert end-effector pose list to joint position list using series of
InverseKinematics problems. Note that q is 9-dimensional because the last 2 dimensions
contain gripper joints, but these should not matter to the constraints.
@param: pose_lst (python list): post_lst[i] contains keyframe X_WG at index i.
@return: q_knots (python_list): q_knots[i] contains IK solution that will give f(q_knots[i]) \approx pose_lst[i].
"""
q_knots = []
plant, _ = CreateIiwaControllerPlant()
world_frame = plant.world_frame()
gripper_frame = plant.GetFrameByName("body")
#q_nominal = np.array([ 0., 0.6, 0., -1.75, 0., 1., 0., 0., 0.]) # nominal joint for joint-centering.
q_nominal = np.array(
[-1.57, 0.1, 0.00, -1.2, 0.00, 1.60, 0.00, 0.00, 0.00])
def AddOrientationConstraint(ik, R_WG, bounds):
"""Add orientation constraint to the ik problem. Implements an inequality
constraint where the axis-angle difference between f_R(q) and R_WG must be
within bounds. Can be translated to:
ik.prog().AddBoundingBoxConstraint(angle_diff(f_R(q), R_WG), -bounds, bounds)
"""
ik.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WG,
frameBbar=gripper_frame,
R_BbarB=RotationMatrix(),
theta_bound=bounds)
def AddPositionConstraint(ik, p_WG_lower, p_WG_upper):
"""Add position constraint to the ik problem. Implements an inequality
constraint where f_p(q) must lie between p_WG_lower and p_WG_upper. Can be
translated to
ik.prog().AddBoundingBoxConstraint(f_p(q), p_WG_lower, p_WG_upper)
"""
ik.AddPositionConstraint(frameA=world_frame,
frameB=gripper_frame,
p_BQ=np.zeros(3),
p_AQ_lower=p_WG_lower,
p_AQ_upper=p_WG_upper)
for i in range(len(pose_lst)):
# if i % 100 == 0:
# print(i)
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q() # Get variables for MathematicalProgram
prog = ik.prog() # Get MathematicalProgram
#### Modify here ###############################
X_WG = pose_lst[i]
p_WG = X_WG.translation()
r_WG = X_WG.rotation()
z_slack = 0
degrees_slack = 0
AddPositionConstraint(ik, p_WG - np.array([0, 0, z_slack]),
p_WG + np.array([0, 0, z_slack]))
AddOrientationConstraint(ik, r_WG, degrees_slack * np.pi / 180)
# initial guess
if i == 0:
prog.SetInitialGuess(q_variables, q_nominal)
diff = q_variables - q_nominal
else:
prog.SetInitialGuess(q_variables, q_knots[i - 1])
diff = q_variables - q_knots[i - 1]
prog.AddCost(np.sum(diff.dot(diff)))
################################################
result = Solve(prog)
if not result.is_success():
visualize_transform(meshcat,
"FAIL",
X_WG,
prefix='',
length=0.3,
radius=0.02)
print(f"Failed at {i}")
break
#raise RuntimeError
tmp = result.GetSolution(q_variables)
q_knots.append(tmp)
return q_knots
def InverseKinPointwise(
p_WQ_start,
p_WQ_end,
duration,
num_knot_points,
q_initial_guess,
InterpolatePosition=None,
InterpolateOrientation=None,
position_tolerance=0.005,
theta_bound=0.005 * np.pi, # 0.9 degrees
is_printing=True):
"""
Calculates a joint space trajectory for iiwa by repeatedly calling IK. The first IK is initialized (seeded)
with q_initial_guess. Subsequent IKs are seeded with the solution from the previous IK.
Positions for point Q (p_EQ) and orientations for the end effector, generated respectively by
InterpolatePosition and InterpolateOrientation, are added to the IKs as constraints.
@param p_WQ_start: The first argument of function InterpolatePosition (defined below).
@param p_WQ_end: The second argument of function InterpolatePosition (defined below).
@param duration: The duration of the trajectory returned by this function in seconds.
@param num_knot_points: number of knot points in the trajectory.
@param q_initial_guess: initial guess for the first IK.
@param InterpolatePosition: A function with signature (start, end, num_knot_points, i). It returns
p_WQ, a (3,) numpy array which describes the desired position of Q at knot point i in world frame.
@param InterpolateOrientation: A function with signature
@param position_tolerance: tolerance for IK position constraints in meters.
@param theta_bound: tolerance for IK orientation constraints in radians.
@param is_printing: whether the solution results of IKs are printed.
@return: qtraj: a 7-dimensional cubic polynomial that describes a trajectory for the iiwa arm.
@return: q_knots: a (n, num_knot_points) numpy array (where n = plant.num_positions()) that stores solutions
returned by all IKs. It can be used to initialize IKs for the next trajectory.
"""
q_knots = np.zeros((num_knot_points + 1, plant.num_positions()))
q_knots[0] = q_initial_guess
for i in range(num_knot_points):
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q()
# Orientation constraint
R_WL7_ref = InterpolateOrientation(i, num_knot_points)
ik.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WL7_ref,
frameBbar=l7_frame,
R_BbarB=RotationMatrix.Identity(),
theta_bound=theta_bound)
# Position constraint
p_WQ = InterpolatePosition(p_WQ_start, p_WQ_end, num_knot_points, i)
ik.AddPositionConstraint(frameB=l7_frame,
p_BQ=p_L7Q,
frameA=world_frame,
p_AQ_lower=p_WQ - position_tolerance,
p_AQ_upper=p_WQ + position_tolerance)
prog = ik.prog()
# use the robot posture at the previous knot point as
# an initial guess.
prog.SetInitialGuess(q_variables, q_knots[i])
result = prog.Solve()
if is_printing:
print i, ": ", result
q_knots[i + 1] = prog.GetSolution(q_variables)
t_knots = np.linspace(0, duration, num_knot_points + 1)
q_knots_kuka = GetKukaQKnots(q_knots)
qtraj = PiecewisePolynomial.Cubic(t_knots, q_knots_kuka.T, np.zeros(7),
np.zeros(7))
return qtraj, q_knots
def GenerateIiwaTrajectoriesAndGripperSetPoints(X_WO, is_printing=True):
"""
:param X_WO: the pose of the foam brick in world frame.
:param is_printing: set to True to print IK solution results.
:return: 1. a list of joint space trajectories.
2. a list of gripper set points.
The two lists must have the same length.
"""
station = ManipulationStation()
station.SetupManipulationClassStation()
station.Finalize()
plant = station.get_controller_plant()
# Go to p_WQ_home
ik_iiwa = inverse_kinematics.InverseKinematics(plant)
world_frame = plant.world_frame()
l7_frame = plant.GetFrameByName("iiwa_link_7")
theta_bound = 0.005 * np.pi
R_WL7 = RollPitchYaw(0, np.pi * 5 / 6, 0).ToRotationMatrix()
ik_iiwa.AddOrientationConstraint(
frameAbar=world_frame, R_AbarA=R_WL7, # rotate end effector to R_WL7 angle
frameBbar=l7_frame, R_BbarB=RotationMatrix(),
theta_bound=theta_bound)
ik_iiwa.AddPositionConstraint(
frameB=l7_frame, p_BQ=p_L7Q,
frameA=world_frame,
p_AQ_lower=p_WQ_home - 0.01, #p_WQ_home corresponds to q_val_0's pos
p_AQ_upper=p_WQ_home + 0.01)
prog = ik_iiwa.prog()
prog.SetInitialGuess(ik_iiwa.q(), np.zeros(7))
result = mp.Solve(prog)
if is_printing:
print(result.get_solution_result())
q_val_0 = result.GetSolution(ik_iiwa.q())
qtraj_leave_home = ConnectPointsWithCubicPolynomial(q_home, q_val_0, 2.0)
# close fingers
q_knots = np.zeros((2, 7))
q_knots[0] = qtraj_leave_home.value(qtraj_leave_home.end_time()).squeeze()
q_knots[1] = q_knots[0]
qtraj_open_gripper = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)
# Complete your pick and place trajectories.
def MoveEndEffectorAlongStraightLine(
p_WQ_start, p_WQ_end, duration, get_orientation, q_initial_guess, num_knots):
"""
p_WQ_start is the starting position of point Q (the point Q we defined earlier!)
p_WQ_end is the end position of point Q.
duration is the duration of the trajectory measured in seconds.
get_orientation(i, n) is a function that returns the interpolated orientation R_WL7 at the i-th knot point out of n knot points. It can simply return a constant orientation, but changing the orientation of link 7 should be helpful for placing the object on the shelf.
num_knots is the number of knot points in the trajectory. Generally speaking,the larger num_knots is, the smoother the trajectory, but solving for more knot points also takes longer. You can start by setting num_knots to 10, which should be sufficient for the task.
"""
t_knots = np.linspace(0, duration, num_knots+1)
q_knots = np.zeros((num_knots, plant.num_positions()))
q_knots[0] = q_initial_guess
n = len(q_initial_guess)
for i in range(num_knots):
ik = inverse_kinematics.InverseKinematics(plant)
prog = ik.prog()
p_AQ = p_WQ_start + (p_WQ_end - p_WQ_start) * i / (num_knots - 1)
ik.AddPositionConstraint(frameB=l7_frame, p_BQ=p_L7Q, frameA=world_frame, p_AQ_lower=p_AQ - 0.01, p_AQ_upper=p_AQ + 0.01) # interpolate between p_WQ_start and p_WQ_end
ik.AddOrientationConstraint(frameAbar=world_frame, R_AbarA=get_orientation(i, num_knots), frameBbar=l7_frame, R_BbarB=RotationMatrix(), theta_bound=theta_bound) # call get_orientation(i, num_knots).
if i == 0:
prog.SetInitialGuess(ik.q(), q_initial_guess)
else:
# This is very important for the smoothness of the whole trajectory.
prog.SetInitialGuess(ik.q(), q_knots[i - 1])
result = mp.Solve(ik.prog())
assert result.get_solution_result() == mp.SolutionResult.kSolutionFound
q_knots[i] = result.GetSolution(ik.q())
return PiecewisePolynomial.Cubic(t_knots[1:], q_knots.T, np.zeros(n), np.zeros(n)) # return a cubic spline that connects all q_knots.
q_curr = q_knots[1]
p_mid = np.array([0.53, 0.02, 0.38])
a_box = 0.14
tilt_link_7 = lambda i, n: RollPitchYaw(a_box*i/(n-1), 5/6*np.pi, 0).ToRotationMatrix()
RPY = RollPitchYaw(a_box, np.pi * 5 / 6, 0).ToRotationMatrix()
qtraj_down = MoveEndEffectorAlongStraightLine(p_WQ_home, p_mid, 3, tilt_link_7, q_curr, 10)
q_curr = qtraj_down.value(qtraj_down.end_time()).squeeze()
p_box = np.array([0.52, 0.033, 0.18])
qtraj_down2 = MoveEndEffectorAlongStraightLine(p_mid, p_box, 4, lambda i, n: RPY, q_curr, 10)
q_curr = qtraj_down2.value(qtraj_down2.end_time()).squeeze()
p_up = np.array([0.53, 0.033, 0.38])
qtraj_up = MoveEndEffectorAlongStraightLine(p_box, p_up, 4, lambda i, n: RPY, q_curr, 10)
q_curr = qtraj_up.value(qtraj_up.end_time()).squeeze()
p_down = np.array([0.52, 0.033, 0.21])
qtraj_backdown = MoveEndEffectorAlongStraightLine(p_up, p_down, 4, lambda i, n: RPY, q_curr, 10)
q_curr = qtraj_backdown.value(qtraj_backdown.end_time()).squeeze()
qtraj_return = ConnectPointsWithCubicPolynomial(q_curr, q_home, 3.0)
q_traj_list = [qtraj_leave_home,
qtraj_open_gripper,
qtraj_down,
qtraj_down2,
qtraj_up,
qtraj_backdown,
qtraj_return
]
gripper_setpoint_list = [0.01, 0.1, 0.1, 0.1, 0.029, 0.029, 0.1]
return q_traj_list, gripper_setpoint_list
def InverseKinPointwise(InterpolatePosition,
InterpolateOrientation,
p_L7Q,
duration,
num_knot_points,
q_initial_guess,
position_tolerance=0.005,
theta_bound=0.005 * np.pi):
"""
Calculates a joint space trajectory for iiwa by repeatedly calling IK.
The returned trajectory has (num_knot_points) knot points. To improve
the continuity of the trajectory, the IK from which q_knots[i] is
solved is initialized with q_knots[i-1].
Positions for point Q (p_EQ) and orientations for the end effector, generated
respectively by InterpolatePosition and InterpolateOrientation,
are added to the IKs as constraints.
:param InterpolatePosition: A function with signature (tau) with
0 <= tau <= 1.
It returns p_WQ, a (3,) numpy array which describes the desired
position of Q. For example, to get p_WQ for knot point i, use
tau = i / (num_knot_points - 1).
:param InterpolateOrientation: A function with the same signature as
InterpolatePosition.
It returns R_WL7, a RotationMatrix which describes the desired
orientation of frame L7.
:param num_knot_points: number of knot points in the trajectory.
:param q_initial_guess: initial guess for the first IK.
:param position_tolerance: tolerance for IK position constraints in meters.
:param theta_bound: tolerance for IK orientation constraints in radians.
:param is_printing: whether the solution results of IKs are printed.
:return: qtraj: a 7-dimensional cubic polynomial that describes a
trajectory for the iiwa arm.
:return: q_knots: a (n, num_knot_points) numpy array (where n =
plant.num_positions()) that stores solutions returned by all IKs. It
can be used to initialize IKs for the next trajectory.
"""
q_knots = np.zeros((num_knot_points, plant.num_positions()))
################### Your code here ###################
for i in range(num_knot_points):
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q()
# Orientation constraint
R_WL7_ref = InterpolateOrientation(i / (num_knot_points - 1))
ik.AddOrientationConstraint(
frameAbar=world_frame, R_AbarA=R_WL7_ref,
frameBbar=l7_frame, R_BbarB=RotationMatrix.Identity(),
theta_bound=theta_bound)
# Position constraint
p_WQ = InterpolatePosition(i / (num_knot_points - 1))
ik.AddPositionConstraint(
frameB=l7_frame, p_BQ=p_L7Q,
frameA=world_frame,
p_AQ_lower=p_WQ - position_tolerance,
p_AQ_upper=p_WQ + position_tolerance)
prog = ik.prog()
# use the robot configuration at the previous knot point as
# an initial guess.
if i > 0:
prog.SetInitialGuess(q_variables, q_knots[i-1])
else:
prog.SetInitialGuess(q_variables, q_initial_guess)
result = mp.Solve(prog)
# throw if no solution found.
if result.get_solution_result() != SolutionResult.kSolutionFound:
print(i, result.get_solution_result())
raise RuntimeError
q_knots[i] = result.GetSolution(q_variables)
t_knots = np.linspace(0, duration, num_knot_points)
qtraj = PiecewisePolynomial.Cubic(
t_knots, q_knots.T, np.zeros(7), np.zeros(7))
return qtraj, q_knots
########################################################
raise NotImplementedError
