def runIk(self, fields): self.checkJointNameOrder(fields) ikoptions = self.makeIkOptions(fields) constraints = self.makeConstraints(fields) # todo # set joint limits on rigidBodyTree from fields.jointLimits q_nom = np.asarray(fields.poses[fields.nominalPose], dtype=float) q_seed = np.asarray(fields.poses[fields.seedPose], dtype=float) if rbt is RigidBodyTreeCompatOld: results = pydrakeik.inverseKinSimple(self.rigidBodyTree, q_seed, q_nom, constraints, ikoptions) q_end = results.q_sol info = results.INFO else: results = pydrakeik.InverseKin(self.rigidBodyTree, q_seed, q_nom, constraints, ikoptions) q_end = results.q_sol[0] info = results.info[0] #for i in xrange(len(results.infeasible_constraints)): # print 'infeasible constraint:', results.infeasible_constraints[i] # convert shape from Nx1 to N q_end.shape = q_end.shape[0] return q_end, info
def setup_scene(rbt): AddFlatTerrainToWorld(rbt) instances = [] num_objects = np.random.randint(10) + 1 for i in range(num_objects): object_ind = np.random.randint(len(objects.keys())) pose = np.zeros(6) pose[0:2] = (np.random.random(2) - 0.5) * 0.05 pose[2] = np.random.random() * 0.1 + 0.05 pose[3:6] = np.random.random(3) * np.pi * 2. object_init_frame = RigidBodyFrame("object_init_frame", rbt.world(), pose[0:3], pose[3:6]) object_path = objects[objects.keys()[object_ind]]["model_path"] if object_path.split(".")[-1] == "urdf": AddModelInstanceFromUrdfFile(object_path, FloatingBaseType.kRollPitchYaw, object_init_frame, rbt) else: AddModelInstancesFromSdfString( open(object_path).read(), FloatingBaseType.kRollPitchYaw, object_init_frame, rbt) instances.append({ "class": objects.keys()[object_ind], "pose": pose.tolist() }) rbt.compile() # Project arrangement to nonpenetration with IK constraints = [] constraints.append( ik.MinDistanceConstraint(model=rbt, min_distance=0.01, active_bodies_idx=list(), active_group_names=set())) for body_i in range(2, 1 + num_objects): constraints.append( ik.WorldPositionConstraint(model=rbt, body=body_i, pts=np.array([0., 0., 0.]), lb=np.array([-0.5, -0.5, 0.0]), ub=np.array([0.5, 0.5, 0.5]))) q0 = np.zeros(rbt.get_num_positions()) options = ik.IKoptions(rbt) options.setDebug(True) options.setMajorIterationsLimit(10000) options.setIterationsLimit(100000) results = ik.InverseKin(rbt, q0, q0, constraints, options) qf = results.q_sol[0] info = results.info[0] print "Projected with info %d" % info return qf, instances
def ComputeTargetPosture(self, x_seed, target_manipuland_pose, backoff_distance=0.0): q_des_full = np.zeros(self.nq) q_des_full[self.nq - 3:] = target_manipuland_pose desired_positions = self.transform_grasp_points_manipuland(q_des_full) desired_normals = self.transform_grasp_normals_manipuland(q_des_full) desired_positions -= backoff_distance * desired_normals # back off a bit desired_angles = [ math.atan2(desired_normals[1, k], desired_normals[0, k]) for k in range(desired_normals.shape[1]) ] constraints = [] # Constrain the fingertips to lie on the grasp points, # facing approximately along the grasp angles. for i in range(len(self.grasp_points)): constraints.append( ik.WorldPositionConstraint( self.hand, self.finger_link_indices[self.grasp_finger_assignments[i] [0]], self.grasp_finger_assignments[i][1], desired_positions[:, i] - 0.01, # lb desired_positions[:, i] + 0.01) # ub ) constraints.append( ik.WorldEulerConstraint( self.hand, self.finger_link_indices[self.grasp_finger_assignments[i] [0]], [-0.01, -0.01, desired_angles[i] - 1.0], # lb [0.01, 0.01, desired_angles[i] + 1.0]) # ub ) posture_constraint = ik.PostureConstraint(self.hand) # Disambiguate the continuous rotation joint angle choices # for the hand. posture_constraint.setJointLimits(np.arange(self.nq - 3), -np.ones(self.nq - 3) * math.pi, np.ones(self.nq - 3) * math.pi) # Constrain the manipuland to be in the target position. posture_constraint.setJointLimits( [self.nq - 3, self.nq - 2, self.nq - 1], target_manipuland_pose - 0.01, target_manipuland_pose + 0.01) constraints.append(posture_constraint) options = ik.IKoptions(self.hand) results = ik.InverseKin(self.hand, x_seed[0:self.nq], self.x_nom[0:self.nq], constraints, options) return results.q_sol[0], results.info[0]
def plan_ee_configuration(rbt, q0, qseed, ee_pose, ee_body, ee_point, allow_collision=True, active_bodies_idx=list(), euler_limits=0.3): ''' Performs IK for a single point in time to get the Kuka's gripper to a specified pose in space. ''' nq = rbt.get_num_positions() controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] free_config_inds, constrained_config_inds = \ kuka_utils.extract_position_indices(rbt, controlled_joint_names) # Assemble IK constraints constraints = [] if not allow_collision: constraints.append( ik.MinDistanceConstraint(model=rbt, min_distance=1E-6, active_bodies_idx=active_bodies_idx, active_group_names=set())) # Constrain the non-searched-over joints posture_constraint = ik.PostureConstraint(rbt) posture_constraint.setJointLimits(constrained_config_inds, q0[constrained_config_inds] - 0.001, q0[constrained_config_inds] + 0.001) constraints.append(posture_constraint) # Constrain the ee frame to lie on the target point # facing in the target orientation constraints.append( ik.WorldPositionConstraint(rbt, ee_body, ee_point, ee_pose[0:3] - 0.01, ee_pose[0:3] + 0.01)) constraints.append( ik.WorldEulerConstraint(rbt, ee_body, ee_pose[3:6] - euler_limits, ee_pose[3:6] + euler_limits)) options = ik.IKoptions(rbt) results = ik.InverseKin(rbt, q0, qseed, constraints, options) #print "plan ee config info %d" % results.info[0] return results.q_sol[0], results.info[0]
def testPostureConstraint(self): r = pydrake.rbtree.RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/Pendulum/Pendulum.urdf")) q = -0.9 posture_constraint = ik.PostureConstraint(r) posture_constraint.setJointLimits(np.array([[6]], dtype=np.int32), np.array([[q]]), np.array([[q]])) # Choose a seed configuration (randomly) and a nominal configuration # (at 0) q_seed = np.vstack((np.zeros((6, 1)), 0.8147)) q_nom = np.vstack((np.zeros((6, 1)), 0.)) options = ik.IKoptions(r) results = ik.InverseKin( r, q_seed, q_nom, [posture_constraint], options) self.assertEqual(results.info[0], 1) self.assertAlmostEqual(results.q_sol[0][6], q, 1e-9) # Run the tests again both pointwise and as a trajectory to # validate the interfaces. t = np.array([0., 1.]) q_seed_array = np.transpose(np.array([q_seed, q_seed]))[0] q_nom_array = np.transpose(np.array([q_nom, q_nom]))[0] results = ik.InverseKinPointwise(r, t, q_seed_array, q_nom_array, [posture_constraint], options) self.assertEqual(len(results.info), 2) self.assertEqual(len(results.q_sol), 2) self.assertEqual(results.info[0], 1) # The pointwise result will go directly to the constrained # value. self.assertAlmostEqual(results.q_sol[0][6], q, 1e-9) self.assertEqual(results.info[1], 1) self.assertAlmostEqual(results.q_sol[1][6], q, 1e-9) results = ik.InverseKinTraj(r, t, q_seed_array, q_nom_array, [posture_constraint], options) self.assertEqual(len(results.info), 1) self.assertEqual(len(results.q_sol), 2) self.assertEqual(results.info[0], 1) # The trajectory result starts at the initial value and moves # to the constrained value. self.assertAlmostEqual(results.q_sol[0][6], q_seed[6], 1e-9) self.assertAlmostEqual(results.q_sol[1][6], q, 1e-9)
def solveAndDraw(constraints): #print q_seed.shape global results global constraintList constraintList = constraints results = pydrakeik.InverseKin(robot, q_seed, q_seed, constraints, options) pose = results.q_sol[0] #print results.info[0] pose = pose[jointIndexMap] robotSystem.robotStateJointController.setPose('pose_end', pose)
def plan_grasping_configuration(rbt, q0, target_ee_pose): ''' Performs IK for a single point in time to get the Kuka's gripper to a specified pose in space. ''' nq = rbt.get_num_positions() q_des_full = np.zeros(nq) controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] free_config_inds, constrained_config_inds = \ kuka_utils.extract_position_indices(rbt, controlled_joint_names) # Assemble IK constraints constraints = [] # Constrain the non-searched-over joints posture_constraint = ik.PostureConstraint(rbt) posture_constraint.setJointLimits(constrained_config_inds, q0[constrained_config_inds] - 0.01, q0[constrained_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain the ee frame to lie on the target point # facing in the target orientation ee_frame = rbt.findFrame("iiwa_frame_ee").get_frame_index() constraints.append( ik.WorldPositionConstraint(rbt, ee_frame, np.zeros( (3, 1)), target_ee_pose[0:3] - 0.01, target_ee_pose[0:3] + 0.01)) constraints.append( ik.WorldEulerConstraint(rbt, ee_frame, target_ee_pose[3:6] - 0.01, target_ee_pose[3:6] + 0.01)) options = ik.IKoptions(rbt) results = ik.InverseKin(rbt, q0, q0, constraints, options) print results.q_sol, "info %d" % results.info[0] return results.q_sol[0], results.info[0]
def project_rbt_to_nearest_feasible_on_table(self, rbt, q0): # Project arrangement to nonpenetration with IK constraints = [] q0 = np.clip(q0, rbt.joint_limit_min, rbt.joint_limit_max) constraints.append(ik.MinDistanceConstraint( model=rbt, min_distance=1E-3, active_bodies_idx=self.manipuland_body_indices + self.tabletop_indices, active_group_names=set())) options = ik.IKoptions(rbt) options.setMajorIterationsLimit(10000) options.setIterationsLimit(100000) options.setQ(np.eye(rbt.get_num_positions())*1E6) results = ik.InverseKin( rbt, q0, q0, constraints, options) qf = results.q_sol[0] info = results.info[0] print "Projected to feasibility with info %d" % info return qf
def solveIK(self, target): constraints = list() constraints.append( ik.WorldGazeTargetConstraint( self.robot, self.bodies[self.config["optical_frame"]], # Body to constrain np.array([0.0, 0.0, 0.1]).reshape(3, 1), # Gaze axis target.reshape(3, 1), # Gaze target (world frame) np.zeros([3, 1]), # Gaze origin (body frame) self.config["cone_threshold"] # Cone threshold )) q_seed = self.robot.getZeroConfiguration() options = ik.IKoptions(self.robot) result = ik.InverseKin(self.robot, q_seed, q_seed, constraints, options) if result.info[0] != 1: print "Outside joint limits! (Result = {})".format(result.info[0]) # If we're out of bounds, result is the best-effort solution return result.q_sol[0][:2] # Only return head pose
def runIK(self, pose_mat): # Set up constraints constraints = list() xyz = transf.translation_from_matrix(pose_mat) pos_delta = self.config["pose_delta"] constraints.append( ik.WorldPositionConstraint( self.robot, self.robot.bodies["iiwa_tool_frame"], # Frame of reference np.array([0.0, 0.0, 0.0]), # Point to constrain (in frame of reference) xyz - pos_delta, # Lower bound xyz + pos_delta # Upper bound )) rpy = np.array(transf.euler_from_matrix(pose_mat)) quat_tol = self.config["quat_tol"] constraints.append( ik.WorldEulerConstraint( self.robot, self.robot.bodies["iiwa_tool_frame"], # Frame of reference rpy - quat_tol, # Lower bound rpy + quat_tol # Upper bound )) # Run the IK q_seed = self.robot.getZeroConfiguration() options = ik.IKoptions(self.robot) result = ik.InverseKin(self.robot, q_seed, q_seed, constraints, options) if result.info[0] == 1: return result.q_sol[0][2:], True else: return [], False
np.array([np.nan, np.nan, 0.0])), ik.WorldPositionConstraint(robot, base_body_id, np.array([1.0, 0.0, 0.0]), np.array([np.nan, np.nan, 0.0]), np.array([np.nan, np.nan, 0.0])), ik.WorldPositionConstraint(robot, base_body_id, np.array([0.0, 1.0, 0.0]), np.array([np.nan, np.nan, 0.0]), np.array([np.nan, np.nan, 0.0])), # This constraint exactly specifies the desired position of the # hand frame we defined earlier. ik.WorldPositionConstraint(robot, hand_frame_id, np.array([0.0, 0.0, 0.0]), np.array([0.5, 0.0, 0.6]), np.array([0.5, 0.0, 0.6])), # And this specifies the orientation of that frame ik.WorldEulerConstraint(robot, hand_frame_id, np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.0, 0.0])) ] q_seed = robot.getZeroConfiguration() options = ik.IKoptions(robot) results = ik.InverseKin(robot, q_seed, q_seed, constraints, options) # Each entry (only one is present in this case, since InverseKin() # only returns a single result) in results.info gives the output # status of SNOPT. info = 1 is good, anything less than 10 is OK, and # any info >= 10 indicates an infeasibility or failure of the # optimizer. assert results.info[0] == 1 print(repr(results.q_sol[0]))
def computePoses(self): # Precompute poses head_poses = self.computeHeadPoses() plate_poses = self.computePlatePoses() print "Total head poses:", len(head_poses) print "Total plate poses:", len(plate_poses) print if self.config["save_ik"]: filename = self.config["ik_save_path"] print "IK save path: {}".format(filename) if os.path.isfile(filename): self.config["save_ik"] = False print "WARNING! IK save path already exists. IK saving disabled" print print "Performing IK..." # Solve all the IK all_poses = list() for i, head_pose in enumerate(head_poses): for plate_pose in plate_poses: # Generate constraints and solve IK constraints = list() # Constrain the head pose head_constraint = ik.PostureConstraint( self.robot ) head_constraint.setJointLimits( np.array([self.positions['ptu_pan'], self.positions['ptu_tilt']], dtype=np.int32).reshape(2, 1), head_pose - self.config['head_pose_tol'], head_pose + self.config['head_pose_tol'] ) constraints.append(head_constraint) # Set the calibration plate position constraints.append( ik.RelativePositionConstraint ( self.robot, np.zeros([3,1]), # Point relative to Body A (calibration target) plate_pose - self.config['arm_pos_tol'], # Lower bound relative to Body B (optical frame) plate_pose + self.config['arm_pos_tol'], # Upper bound relative to Body B (optical frame) self.bodies['calibration_target'], # Body A self.bodies[self.config['optical_frame']], # Body B np.concatenate([ # Transform from B to reference point np.zeros(3), # Translation (identity) np.array([1, 0, 0, 0]) # Rotation (identity) ]).reshape(7, 1) )) # Set the calibration plate orientation constraints.append( ik.RelativeGazeDirConstraint ( self.robot, self.bodies['calibration_target'], # Body A self.bodies[self.config['optical_frame']], # Body B np.array([0, 0, 1], dtype=np.float64).reshape(3,1), # Body A axis np.array([0, 0, -1], dtype=np.float64).reshape(3,1), # Body B axis self.config['gaze_dir_tol'] # Cone tolerance in radians )) # Avoid self-collision constraints.append( ik.MinDistanceConstraint ( self.robot, self.config['collision_min_distance'], # Minimum distance between bodies list(), # Active bodies (empty set means all bodies) set() # Active collision groups (not filter groups! Empty set means all) )) # Actually solve the IK! # Since we're solving ahead of time, just use the zero as seed # NOTE: Could possibly track last solution as seed... q_seed = self.robot.getZeroConfiguration() options = ik.IKoptions(self.robot) result = ik.InverseKin(self.robot, q_seed, q_seed, constraints, options) if result.info[0] != 1: pass else: # Tuple: head_pose, arm_pose all_poses.append((result.q_sol[0][0:2],result.q_sol[0][2:])) if self.config["save_ik"]: with open(self.config["ik_save_path"],'a') as fp: np.savetxt(fp, result.q_sol[0][None, :], delimiter=',') # Show status info after every head pose attempted = (i+1) * len(plate_poses) success = len(all_poses) total = len(head_poses)*len(plate_poses) completion = attempted*100 / total print "[{:>3d}%] {}/{} solutions found".format(completion, success, attempted) return all_poses
def solveIK(self, pose, target): """ pose: np.array([latitude, longitude, depth]) target: np.array([x, y, z, quat[4]]) of view target in "world" frame """ # Duration of motion for solver dur = self.config["trajectory_duration"] constraints = list() # Constrain the gaze constraints.append( ik.WorldGazeTargetConstraint( self.robot, self.bodies[self.config["optical_frame"]], # Body to constrain np.array([0.0, 0.0, 1.0]).reshape(3, 1), # Gaze axis target[0:3, None], # Gaze target (world frame) np.zeros([3, 1]), # Gaze origin (body frame) self.config["cone_threshold"] # Cone threshold # np.array([dur, dur]).reshape([2,1]) # Valid times )) # Constrain the position # NOTE: Due to weirdness with the Aruco tag, Y is up and -Z is front # So, longitude rotates around Y # latitude rotates around -X # depth is in positive Z # ... This is easier just doing trig by hand camera_pose = np.array([ pose[2] * math.cos(pose[0]) * math.sin(pose[1]), # X pose[2] * math.sin(pose[0]), # Y pose[2] * math.cos(pose[0]) * math.cos(pose[1]) # Z ]) print "Pose: ", pose print "Camera: ", camera_pose constraints.append( ik.RelativePositionConstraint( self.robot, np.zeros([3, 1]), # Point relative to Body A (optical frame) camera_pose - self. config['pose_tol'], # Lower bound relative to Body B (target) camera_pose + self. config['pose_tol'], # Upper bound relative to Body B (target) self.bodies[self.config['optical_frame']], # Body A self.bodies["world"], # Body B target.reshape(7, 1) # Transform from B to reference point )) # Avoid collisions # NOTE: How do we avoid colliding with the table? # * Table needs to be in the RBT # constraints.append( ik.MinDistanceConstraint ( self.robot, # self.config['collision_min_distance'], # Minimum distance between bodies # list(), # Active bodies (empty set means all bodies) # set() # Active collision groups (not filter groups! Empty set means all) # )) # Seed with current configuration (eventually!) q_seed = self.robot.getZeroConfiguration() options = ik.IKoptions(self.robot) result = ik.InverseKin(self.robot, q_seed, q_seed, constraints, options) if result.info[0] != 1: print "IK Failed! Result:", result.info return None print return result.q_sol[0]