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