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