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]
