def test_compare_and_concatenate(self):
x = np.array([[10.], [20.], [30.]]).transpose()
pp1 = PiecewisePolynomial.FirstOrderHold([0., 1., 2.], x)
pp2 = PiecewisePolynomial.FirstOrderHold([2., 3., 4.], x)
self.assertTrue(pp1.isApprox(other=pp1, tol=1e-14))
pp1.ConcatenateInTime(other=pp2)
self.assertEqual(pp1.end_time(), 4.)
def generate_rpyxyz_and_gripper_trajectory(self, mbp_context):
start_position = self.mbp.EvalBodyPoseInWorld(
mbp_context, self.target_body).translation()
adjusted_goal_position = self.goal_position.copy()
if self.offset_body:
adjusted_goal_position += self.mbp.EvalBodyPoseInWorld(
mbp_context, self.offset_body).translation()
start_ee_pose = self.mbp.EvalBodyPoseInWorld(
mbp_context, self.mbp.GetBodyByName("iiwa_link_7"))
grasp_offset = np.array(
[0.0, 0., 0.225]) # Amount *all* target points are shifted up
up_offset = np.array(
[0., 0., 0.1]) # Additional amount to lift objects off of table
# Timing:
# 0 : t_reach: move over object
# t_reach : t_touch: move down to object
# t_touch : t_grasp: close gripper
# t_grasp : t_lift : pick up object
# t_lift : t_move : move object over destination
# t_move : t_down : move object down
# t_down : t_drop : open gripper
# t_drop : t_done : rise back up
t_each = 0.25
t_reach = 0. + t_each
t_touch = t_reach + t_each
t_grasp = t_touch + t_each
t_lift = t_grasp + t_each
t_move = t_lift + t_each
t_down = t_move + t_each
t_drop = t_down + t_each
t_done = t_drop + t_each
ts = np.array([
0., t_reach, t_touch, t_grasp, t_lift, t_move, t_down, t_drop,
t_done
])
ee_xyz_knots = np.vstack([
start_ee_pose.translation() - grasp_offset,
start_position + up_offset,
start_position,
start_position,
start_position + up_offset,
adjusted_goal_position + up_offset,
adjusted_goal_position,
adjusted_goal_position,
adjusted_goal_position + up_offset,
]).T
ee_xyz_knots += np.tile(grasp_offset, [len(ts), 1]).T
ee_rpy_knots = np.array([-np.pi, 0., np.pi / 2.])
ee_rpy_knots = np.tile(ee_rpy_knots, [len(ts), 1]).T
ee_rpyxyz_knots = np.vstack([ee_rpy_knots, ee_xyz_knots])
ee_rpyxyz_traj = PiecewisePolynomial.FirstOrderHold(
ts, ee_rpyxyz_knots)
gripper_knots = np.array([[0.1, 0.1, 0.1, 0., 0., 0., 0., 0.1, 0.1]])
gripper_traj = PiecewisePolynomial.FirstOrderHold(ts, gripper_knots)
return ee_rpyxyz_traj, gripper_traj
def urdfViz(plant):
"""
Visualize the position/orientation of the vehicle along a trajectory using the PlanarRigidBodyVisualizer and
a basic .urdf file representing the vehicle, start, goal, and state constraints.
"""
tree = RigidBodyTree(FindResource("/notebooks/6832-code/ben_uav.urdf"),
FloatingBaseType.kRollPitchYaw)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-1, 15], ylim=[-0.5, 2.5])
tf = plant.ttraj[-1]
times = np.linspace(0, tf, 200)
# organize the relevant states for the visualizer
posn = np.zeros((13, len(times)))
for i, t in enumerate(times):
x = plant.xdtraj_poly.value(t)
u = plant.udtraj_poly.value(t)
posn[0:6, i] = 0
posn[6, i] = (x[0] - plant.xdtraj[:,0].min()) / 14 # x
posn[7, i] = 0 # z
posn[8, i] = x[1] # y
posn[9, i] = 0 # yz plane
posn[10, i] = np.pi / 2 - x[4] # pitch down
posn[11, i] = 0 # yaw
posn[12, i] = -u[1] # tail angle
test_poly = PiecewisePolynomial.FirstOrderHold(times, posn)
return vis.animate(test_poly, repeat=False)
def test_trajectory(self):
builder = DiagramBuilder()
visualizer = builder.AddSystem(TestVisualizer(1))
ppt = PiecewisePolynomial.FirstOrderHold(
[0., 1.], [[2., 3.], [2., 1.]])
ani = visualizer.animate(ppt)
self.assertIsInstance(ani, animation.FuncAnimation)
def test_first_order_hold(self):
x = np.array([[1., 2.], [3., 4.], [5., 6.]]).transpose()
pp = PiecewisePolynomial.FirstOrderHold([0., 1., 2.], x)
np.testing.assert_equal(np.array([[2.], [3.]]), pp.value(.5))
deriv = pp.MakeDerivative(derivative_order=1)
np.testing.assert_equal(np.array([[2.], [2.]]), deriv.value(.5))
pp.AppendFirstOrderSegment(time=3., sample=[-0.4, .57])
def buildTrajectorySource(ts, knots):
#good_steps = np.diff(np.hstack([ts_raw, np.array(ts_raw[-1])])) >= 0.001
#ts = ts_raw[good_steps]
#knots = knots_raw[:, good_steps]
ppt = PiecewisePolynomial.FirstOrderHold(ts, knots)
return TrajectorySource(trajectory=ppt,
output_derivative_order=0,
zero_derivatives_beyond_limits=True)
def test_reverse_and_scale_time(self):
x = np.array([[10.], [20.], [30.]]).transpose()
pp = PiecewisePolynomial.FirstOrderHold([0.5, 1., 2.], x)
pp.ReverseTime()
self.assertEqual(pp.start_time(), -2.0)
self.assertEqual(pp.end_time(), -0.5)
pp.ScaleTime(2.0)
self.assertEqual(pp.start_time(), -4.0)
self.assertEqual(pp.end_time(), -1.0)
def make_finger_trajectory(times):
opened = np.array([0.08])
closed = np.array([0.00])
traj_hand_command = PiecewisePolynomial.FirstOrderHold(
[times["initial"], times["pick_start"]], np.hstack([[opened], [opened]]))
traj_hand_command.AppendFirstOrderSegment(times["pick_end"], closed)
traj_hand_command.AppendFirstOrderSegment(times["place_start"], closed)
traj_hand_command.AppendFirstOrderSegment(times["place_end"], opened)
traj_hand_command.AppendFirstOrderSegment(times["postplace"], opened)
return traj_hand_command
def test_slice_and_shift(self):
x = np.array([[10.], [20.], [30.]]).transpose()
pp = PiecewisePolynomial.FirstOrderHold([0., 1., 2.], x)
pp_sub = pp.slice(start_segment_index=1, num_segments=1)
self.assertEqual(pp_sub.get_number_of_segments(), 1)
self.assertEqual(pp_sub.start_time(), 1.)
self.assertEqual(pp_sub.end_time(), 2.)
values_sub = np.array(list(map(pp_sub.value, [1., 2.])))
self.assertTrue((values_sub == [[[20.]], [[30.]]]).all())
pp_sub.shiftRight(10.)
self.assertEqual(pp_sub.start_time(), 11.)
self.assertEqual(pp_sub.end_time(), 12.)
def test_reshape_and_block(self):
t = [0., 1., 2., 3.]
x = np.diag([4., 5., 6., 7.])
pp = PiecewisePolynomial.FirstOrderHold(t, x)
self.assertEqual(pp.rows(), 4)
self.assertEqual(pp.cols(), 1)
pp.Reshape(rows=2, cols=2)
self.assertEqual(pp.rows(), 2)
self.assertEqual(pp.cols(), 2)
pp2 = pp.Block(start_row=0, start_col=0, block_rows=2, block_cols=1)
self.assertEqual(pp2.rows(), 2)
self.assertEqual(pp2.cols(), 1)
def make_gripper_position_trajectory(X_G, times):
""" Constructs a gripper position trajectory from the plan "sketch" """
traj = PiecewisePolynomial.FirstOrderHold(
[times["initial"], times["prepick"]], np.vstack([X_G["initial"].translation(), X_G["prepick"].translation()]).T
# [times["initial"], times["prepick"]], np.vstack([X_G["initial"].translation(), X_G["initial"].translation()]).T
)
traj.AppendFirstOrderSegment(times["pick_start"], X_G["pick"].translation())
traj.AppendFirstOrderSegment(times["pick_end"], X_G["pick"].translation())
traj.AppendFirstOrderSegment(times["postpick"], X_G["prepick"].translation())
traj.AppendFirstOrderSegment(times["clearance"], X_G["clearance"].translation())
traj.AppendFirstOrderSegment(times["preplace"], X_G["preplace"].translation())
traj.AppendFirstOrderSegment(times["place_start"], X_G["place"].translation())
traj.AppendFirstOrderSegment(times["place_end"], X_G["place"].translation())
traj.AppendFirstOrderSegment(times["postplace"], X_G["preplace"].translation())
return traj
def test_trajectory_source(self):
ppt = PiecewisePolynomial.FirstOrderHold(
[0., 1.], [[2., 3.], [2., 1.]])
system = TrajectorySource(trajectory=ppt,
output_derivative_order=0,
zero_derivatives_beyond_limits=True)
context = system.CreateDefaultContext()
output = system.AllocateOutput()
def mytest(input, expected):
context.SetTime(input)
system.CalcOutput(context, output)
self.assertTrue(np.allclose(output.get_vector_data(
0).CopyToVector(), expected))
mytest(0.0, (2.0, 2.0))
mytest(0.5, (2.5, 1.5))
mytest(1.0, (3.0, 1.0))
def test_matrix_trajectories(self):
A0 = np.array([[1, 2, 3], [4, 5, 6]])
A1 = np.array([[7, 8, 9], [10, 11, 12]])
A2 = np.array([[13, 14, 15], [16, 17, 18]])
t = [0., 1., 2.]
pp = dict()
pp["zoh"] = PiecewisePolynomial.ZeroOrderHold(breaks=t,
samples=[A0, A1, A2])
pp["foh"] = PiecewisePolynomial.FirstOrderHold(breaks=t,
samples=[A0, A1, A2])
pp["hermite"] = PiecewisePolynomial.CubicShapePreserving(
breaks=t, samples=[A0, A1, A2], zero_end_point_derivatives=False)
pp["c1"] = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t, samples=[A0, A1, A2], periodic_end=False)
pp["c2"] = PiecewisePolynomial.CubicHermite(
breaks=t,
samples=[A0, A1, A2],
samples_dot=[0 * A0, 0 * A1, 0 * A2])
pp["c3"] = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t,
samples=[A0, A1, A2],
sample_dot_at_start=0 * A0,
sample_dot_at_end=0 * A0)
pp["lagrange"] = PiecewisePolynomial.LagrangeInterpolatingPolynomial(
times=t, samples=[A0, A1, A2])
for name, traj in pp.items():
if name == "lagrange":
self.assertEqual(traj.get_number_of_segments(), 1)
else:
self.assertEqual(traj.get_number_of_segments(), 2)
self.assertEqual(traj.start_time(), 0.)
self.assertEqual(traj.end_time(), 2.)
self.assertEqual(traj.rows(), 2)
self.assertEqual(traj.cols(), 3)
# Check the values for the easy cases:
np.testing.assert_equal(A0, pp["zoh"].value(.5))
np.testing.assert_allclose(0.5 * A0 + 0.5 * A1, pp["foh"].value(.5),
1e-15)
def reconstruct_path(self, node):
"""
Piece together trajectories between nodes in the path to selected node (generally self.best_goal_node).
"""
current = node
utraj, xtraj, ttraj, res, cost = self.plant.trajOptRRT(
current.parent.state,
current.state,
goal=current.goal_node,
verbose=False)
urrt = utraj
xrrt = xtraj
trrt = ttraj
current = current.parent
while current.parent is not None:
utraj, xtraj, ttraj, res, cost = self.plant.trajOptRRT(
current.parent.state,
current.state,
goal=current.goal_node,
verbose=False)
urrt = np.vstack((utraj, urrt))
xrrt = np.vstack((xtraj[:-1, :], xrrt))
trrt = np.hstack((ttraj[:-1], trrt + ttraj.max()))
current = current.parent
# save traj
self.plant.udtraj = urrt
self.plant.xdtraj = xrrt
self.plant.ttraj = trrt
self.plant.mp_result = res
self.plant.udtraj_poly = PiecewisePolynomial.FirstOrderHold(
trrt[0:-1], urrt.T)
self.plant.xdtraj_poly = PiecewisePolynomial.Cubic(trrt, xrrt.T)
return urrt, xrrt, trrt
def test_first_order_hold(self):
x = np.array([[1., 2.], [3., 4.], [5., 6.]]).transpose()
pp = PiecewisePolynomial.FirstOrderHold([0., 1., 2.], x)
np.testing.assert_equal(np.array([[2.], [3.]]), pp.value(.5))
# #TODO
R = 10
dircol.AddRunningCost(R * u[0]**2)
dircol.AddRunningCost(R * u[1]**2)
dircol.AddRunningCost(R * u[2]**2)
P = dircol.NewContinuousVariables(3, "P")
dircol.AddQuadraticCost(Q=np.array([[1., 0., 0.], [0., 1., 0.],
[0., 0., 1.]]),
b=np.array([1., 1., 1.]),
c=1.,
vars=P[:3])
#Initial Trajectory
initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(
[0., 20.], np.column_stack((initial_state, final_state)))
print(initial_x_trajectory)
dircol.SetInitialTrajectory(PiecewisePolynomial(),
initial_x_trajectory)
#Solve the DirCol
result = Solve(dircol)
print("\n\n")
print("THE FUNCTION RETURN")
print(result.is_success())
print("\n\n")
assert (result.is_success())
#Final Trajectory
print("\n\n")
print("THE STATE TRAJ PRINT")
def trajOpt(self, state_initial, dircol=0, second_pass=False):
"""
Perform trajectory optimization, using either direct transcription or direct collocation.
trajOptRRT() is neater and more useful -- just keeping this around to avoid rewriting sims for class project.
"""
# stopwatch for solver time
tsolve_pre = time.time()
(x_goal, V_goal, gamma_goal, q_goal) = (200.0, state_initial[2], 0.0,
0.0)
# number of knot points - proportional to x-distance seems to work well
if not dircol:
N = int(np.floor(0.8 * np.abs(x_goal - state_initial[0])))
else:
N = 30
# optimization problem: variables t_f, u[k], x[k]
mp = MathematicalProgram()
t_f = mp.NewContinuousVariables(1, "t_f")
dt = t_f[0] / N
k = 0
u = mp.NewContinuousVariables(2, "u_%d" % k)
input_trajectory = u
x = mp.NewContinuousVariables(6, "x_%d" % k)
state_trajectory = x
for k in range(1, N):
u = mp.NewContinuousVariables(2, "u_%d" % k)
x = mp.NewContinuousVariables(6, "x_%d" % k)
input_trajectory = np.vstack((input_trajectory, u))
state_trajectory = np.vstack((state_trajectory, x))
x = mp.NewContinuousVariables(6, "x_%d" % N)
state_trajectory = np.vstack((state_trajectory, x))
# for dircol we can use u_N and first-order hold
if dircol:
u = mp.NewContinuousVariables(2, "u_%d" % N)
input_trajectory = np.vstack((input_trajectory, u))
print "Number of decision vars", mp.num_vars()
# cost function: penalize time and control effort
thrust = input_trajectory[:, 0]
elev = input_trajectory[:, 1]
vel = state_trajectory[:, 2]
allvars = np.hstack((t_f[0], thrust, elev, vel))
# TODO: use u of length n+1 for dircol
def totalcost(X):
dt = X[0] / N
u0 = X[1:N + 1]
u1 = X[N + 1:2 * N + 1]
v = X[2 * N + 1:3 * N + 1] # cut last item if dirtrans
return dt * (1.0 * u0.dot(u0) +
1.0 * u1.dot(u1)) + 1.0 * X[0] * (u0.dot(v))
# return dt * (1.0 * u0.dot(u0) + 1.0 * u1.dot(u1) + 10.0 * X[0] * (u0.dot(v)))
mp.AddCost(totalcost, allvars)
# initial state constraint
for i in range(len(state_initial)):
mp.AddLinearConstraint(state_trajectory[0, i] == state_initial[i])
# final state constraint (x position)
mp.AddLinearConstraint(state_trajectory[-1, 0] == x_goal)
# final state constraint (z position) NOTE: range is acceptable
mp.AddLinearConstraint(state_trajectory[-1, 1] <= 1.5)
mp.AddLinearConstraint(state_trajectory[-1, 1] >= 0.5)
# final state constraint (velocity) NOTE: range is acceptable
mp.AddLinearConstraint(state_trajectory[-1, 2] <= 1.5 * V_goal)
mp.AddLinearConstraint(state_trajectory[-1, 2] >= V_goal)
# final state constraint (flight path angle) NOTE: small range here
mp.AddLinearConstraint(
state_trajectory[-1, 3] <= gamma_goal + 1.0 * np.pi / 180.0)
mp.AddLinearConstraint(
state_trajectory[-1, 3] >= gamma_goal - 1.0 * np.pi / 180.0)
# final state constraint (pitch rate)
mp.AddLinearConstraint(state_trajectory[-1, 5] == q_goal)
# input constraints
for i in range(len(input_trajectory[:, 0])):
mp.AddLinearConstraint(input_trajectory[i, 0] >= 0.0)
mp.AddLinearConstraint(
input_trajectory[i, 0] <= 1.2 * self.m * self.g)
mp.AddLinearConstraint(input_trajectory[i, 1] >= -30.0)
mp.AddLinearConstraint(input_trajectory[i, 1] <= 30.0)
# state constraints
for i in range(len(state_trajectory[:, 0])):
# x position
mp.AddLinearConstraint(state_trajectory[i, 0] >= state_initial[0])
mp.AddLinearConstraint(state_trajectory[i, 0] <= x_goal)
# z position
mp.AddLinearConstraint(state_trajectory[i, 1] >= 0.3)
mp.AddLinearConstraint(state_trajectory[i, 1] <= 2.0)
# velocity
mp.AddLinearConstraint(state_trajectory[i, 2] >= 1.0)
mp.AddLinearConstraint(
state_trajectory[i, 2] <= 3.0 * state_initial[2])
# flight path angle
mp.AddLinearConstraint(
state_trajectory[i, 3] >= -30.0 * np.pi / 180.0)
mp.AddLinearConstraint(
state_trajectory[i, 3] <= 30.0 * np.pi / 180.0)
# pitch angle
mp.AddLinearConstraint(
state_trajectory[i, 4] >= -20.0 * np.pi / 180.0)
mp.AddLinearConstraint(
state_trajectory[i, 4] <= 40.0 * np.pi / 180.0)
# pitch rate
mp.AddLinearConstraint(
state_trajectory[i, 5] >= -20.0 * np.pi / 180.0)
mp.AddLinearConstraint(
state_trajectory[i, 5] <= 20.0 * np.pi / 180.0)
# dynamic constraints
if not dircol:
# direct transcription
for j in range(1, N + 1):
dynamic_prop = dt * self.airplaneLongDynamics(
state_trajectory[j - 1, :], input_trajectory[j - 1, :])
for k in range(len(state_initial)):
mp.AddConstraint(
state_trajectory[j, k] == state_trajectory[j - 1, k] +
dynamic_prop[k])
else:
# direct collocation
for j in range(1, N + 1):
x0 = state_trajectory[j - 1, :]
x1 = state_trajectory[j, :]
xdot0 = self.airplaneLongDynamics(x0,
input_trajectory[j - 1, :])
xdot1 = self.airplaneLongDynamics(x1, input_trajectory[j, :])
xc = 0.5 * (x1 + x0) + dt * (xdot0 - xdot1) / 8.0
xdotc = -1.5 * (x0 - x1) / dt - 0.25 * (xdot0 + xdot1)
uc = 0.5 * (input_trajectory[j - 1, :] +
input_trajectory[j, :])
f_xc = self.airplaneLongDynamics(xc, uc)
for k in range(len(state_initial)):
# TODO: why does "==" cause "kUnknownError"?
# mp.AddConstraint(xdotc[k] - f_xc[k] == 0.0)
mp.AddConstraint(xdotc[k] <= f_xc[k] + 0.001)
mp.AddConstraint(xdotc[k] >= f_xc[k] - 0.001)
# allow for warm start of dircol program with output of dirtrans program
if (second_pass) and (self.mp_result == SolutionResult.kSolutionFound):
# warm start using previous output
print 'warm start to traj opt'
t_guess = self.ttraj[-1]
mp.SetInitialGuess(t_f[0], t_guess)
for i in range(len(state_trajectory[:, 0])):
for j in range(len(state_initial)):
mp.SetInitialGuess(state_trajectory[i, j], self.xdtraj[i,
j])
for i in range(N):
mp.SetInitialGuess(input_trajectory[i, 0], self.udtraj[i, 0])
mp.SetInitialGuess(input_trajectory[i, 1], self.udtraj[i, 1])
# time constraints
mp.AddLinearConstraint(t_f[0] <= 1.25 * t_guess)
mp.AddLinearConstraint(t_f[0] >= 0.8 * t_guess)
else:
# initial guesses
t_guess = np.abs(x_goal -
state_initial[0]) / (0.5 *
(V_goal + state_initial[2]))
mp.SetInitialGuess(t_f[0], t_guess)
z_final_dummy = state_initial[1]
theta_final_dummy = state_initial[4]
state_final_dummy = np.array([
x_goal, z_final_dummy, V_goal, gamma_goal, theta_final_dummy,
q_goal
])
for i in range(len(state_trajectory[:, 0])):
state_guess = (
(N - i) / N) * state_initial + (i / N) * state_final_dummy
for j in range(len(state_guess)):
mp.SetInitialGuess(state_trajectory[i, j], state_guess[j])
for i in range(N):
mp.SetInitialGuess(input_trajectory[i, 0],
self.m * self.g / 3.5)
mp.SetInitialGuess(input_trajectory[i, 1], 0.01)
# time constraints
mp.AddLinearConstraint(t_f[0] <= 2.0 * t_guess)
mp.AddLinearConstraint(t_f[0] >= 0.5 * t_guess)
# set SNOPT iteration limit
it_limit = int(max(20000, 40 * mp.num_vars()))
mp.SetSolverOption(SolverType.kSnopt, 'Iterations limit', it_limit)
print("** solver begin with N = %d **" % N)
# solve nonlinear optimization problem (w/SNOPT)
result = mp.Solve()
print result
# convert from symbolic to float
input_trajectory = mp.GetSolution(input_trajectory)
t_f = mp.GetSolution(t_f)
state_trajectory_approx = mp.GetSolution(state_trajectory)
time_array = t_f[0] * np.linspace(0.0, 1.0, (N + 1))
tsolve_post = time.time()
tsolve = tsolve_post - tsolve_pre
solver_id = mp.GetSolverId()
print("** %s solver finished in %.1f seconds **\n" %
(solver_id.name(), tsolve))
print("t_f computed: %.3f seconds" % t_f[0])
# get total cost of solution
if result == SolutionResult.kSolutionFound:
thrust = input_trajectory[:, 0]
elev = input_trajectory[:, 1]
vel = state_trajectory_approx[:, 2]
allvars = np.hstack((t_f[0], thrust, elev, vel))
print("cost computed: %.3f" % totalcost(allvars))
# save traj (this is a bit sloppy and redundant but scripts for visualization currently rely on this)
self.udtraj = input_trajectory
self.xdtraj = state_trajectory_approx
self.ttraj = time_array
self.mp_result = result
# save polynomials of input, state trajectories
if not dircol:
self.udtraj_poly = PiecewisePolynomial.FirstOrderHold(
time_array[0:-1], input_trajectory.T)
else:
self.udtraj_poly = PiecewisePolynomial.FirstOrderHold(
time_array, input_trajectory.T)
self.xdtraj_poly = PiecewisePolynomial.Cubic(time_array,
state_trajectory_approx.T)
return input_trajectory, state_trajectory_approx, time_array
def MakeControlledKukaPlan():
kSdfPath = "drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf"
num_q = 7
rigid_body_tree = RigidBodyTree()
AddModelInstancesFromSdfFile(FindResourceOrThrow(kSdfPath),
FloatingBaseType.kFixed, None,
rigid_body_tree)
zero_conf = rigid_body_tree.getZeroConfiguration()
joint_lb = zero_conf - np.ones(num_q) * 0.01
joint_ub = zero_conf + np.ones(num_q) * 0.01
pc1 = PostureConstraint(rigid_body_tree, np.array([0, 0.5]))
joint_idx = np.arange(num_q)
pc1.setJointLimits(joint_idx, joint_lb, joint_ub)
pos_end = np.array([0.6, 0, 0.325])
pos_lb = pos_end - np.ones(3) * 0.005
pos_ub = pos_end + np.ones(3) * 0.005
wpc1 = WorldPositionConstraint(
rigid_body_tree, rigid_body_tree.FindBodyIndex("iiwa_link_7"),
np.array([0, 0, 0]), pos_lb, pos_ub, np.array([1, 3]))
pc2 = PostureConstraint(rigid_body_tree, np.array([4, 5.9]))
pc2.setJointLimits(joint_idx, joint_lb, joint_ub)
wpc2 = WorldPositionConstraint(
rigid_body_tree, rigid_body_tree.FindBodyIndex("iiwa_link_7"),
np.array([0, 0, 0]), pos_lb, pos_ub, np.array([6, 9]))
joint_position_start_idx = rigid_body_tree.FindChildBodyOfJoint(
"iiwa_joint_2").get_position_start_index()
pc3 = PostureConstraint(rigid_body_tree, np.array([6, 8]))
pc3.setJointLimits(np.array([joint_position_start_idx]),
np.ones(1) * 0.7,
np.ones(1) * 0.8)
kTimes = np.array([0.0, 2.0, 5.0, 7.0, 9.0])
q_seed = np.zeros((rigid_body_tree.get_num_positions(), kTimes.size))
q_norm = np.zeros((rigid_body_tree.get_num_positions(), kTimes.size))
for i in range(kTimes.size):
q_seed[:, i] = zero_conf + 0.1 * np.ones(
rigid_body_tree.get_num_positions())
q_norm[:, i] = zero_conf
ikoptions = IKoptions(rigid_body_tree)
constraint_array = [pc1, wpc1, pc2, pc3, wpc2]
ik_results = InverseKinPointwise(rigid_body_tree, kTimes, q_seed, q_norm,
constraint_array, ikoptions)
q_sol = ik_results.q_sol
ik_info = ik_results.info
infeasible_constraint = ik_results.infeasible_constraints
info_good = True
for i in range(kTimes.size):
print('IK ik_info[{}] = {}'.format(i, ik_info[i]))
if ik_info[i] != 1:
info_good = False
if not info_good:
raise Exception(
"inverseKinPointwise failed to compute a valid solution.")
knots = np.array(q_sol).transpose()
traj_pp = PiecewisePolynomial.FirstOrderHold(kTimes, knots)
return traj_pp
