def testMinDistanceConstraint(self):
model = self.r
min_distance = 1e-2
active_bodies_idx = list()
active_group_name = set()
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.MinDistanceConstraint(model, min_distance, active_bodies_idx,
active_group_name)
# Test that construction doesn't fail with a given timespan.
ik.MinDistanceConstraint(model, min_distance, active_bodies_idx,
active_group_name, tspan)
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 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 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 trajectoryIK(self, start, end):
dur = self.config["trajectory_duration"]
joints = np.array(
[self.positions[joint] for joint in self.joint_names],
dtype=np.int32
).reshape(9, 1)
constraints = list()
# Constrain the start position
start_pose = np.concatenate(start).reshape(9, 1)
start_constraint = ik.PostureConstraint( self.robot,
np.array([0.0, 0.0]).reshape([2,1]) # Active time
)
start_constraint.setJointLimits(
joints,
start_pose - self.config['pose_tol'],
start_pose + self.config['pose_tol']
)
constraints.append(start_constraint)
# Constrain the end position
end_pose = np.concatenate(end).reshape(9, 1)
end_constraint = ik.PostureConstraint( self.robot,
np.array([dur, dur]).reshape([2,1]) # Active time
)
end_constraint.setJointLimits(
joints,
end_pose - self.config['pose_tol'],
end_pose + self.config['pose_tol']
)
constraints.append(end_constraint)
# Constrain against collisions
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)
))
# Prepare times of interest for trajectory solve
times = np.linspace(0, dur, num=self.config['trajectory_count'], endpoint=True)
# Compute cubic interpolation as seed trajectory
x = np.array([0, dur])
y = np.hstack([start_pose, end_pose])
f = CubicSpline(x, y, axis=1, bc_type='clamped')
q_seed = f(times)
# print "q_seed:", q_seed
# Solve the IK problem
options = ik.IKoptions(self.robot)
options.setMajorIterationsLimit(400)
options.setFixInitialState(True)
# print "Iterations limit:", options.getMajorIterationsLimit()
result = ik.InverseKinTraj(self.robot, times, q_seed, q_seed, constraints, options)
if result.info[0] != 1:
print "Could not find safe trajectory!"
print "Start pose:", start_pose.flatten()
print "End pose: ", end_pose.flatten()
print "Result:", result.info
return None
else:
# Clean up: Pack solutions as a matrix, with time as first column
q_sol = np.vstack(result.q_sol)
q_sol = np.insert(q_sol, [0], times[:, None], axis=1)
return q_sol
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 plan_ee_trajectory(rbt,
q0,
traj_seed,
ee_times,
ee_poses,
ee_body,
ee_point,
n_knots,
start_time=0.,
avoid_collision_tspans=[],
active_bodies_idx=list()):
nq = rbt.get_num_positions()
# Create knot points
ts = np.linspace(0., ee_times[-1], n_knots)
# Insert one more for each ee time if not already in there
for t in ee_times:
if t not in ts:
ts = np.insert(ts, np.argmax(ts >= t), t)
print "times ", ts, " for times ", ee_times
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 = []
for tspan in avoid_collision_tspans:
constraints.append(
ik.MinDistanceConstraint(model=rbt,
min_distance=1E-6,
active_bodies_idx=active_bodies_idx,
active_group_names=set(),
tspan=tspan))
print "active for ", tspan
# Make starting constraint sensible if it's out of joint limits
q0 = np.clip(q0, rbt.joint_limit_min, rbt.joint_limit_max)
# Constrain the non-searched-over joints for all time
all_tspan = np.array([0., ts[-1]])
posture_constraint = ik.PostureConstraint(rbt, all_tspan)
posture_constraint.setJointLimits(constrained_config_inds,
q0[constrained_config_inds] - 0.05,
q0[constrained_config_inds] + 0.05)
constraints.append(posture_constraint)
# Constrain all joints to be the initial posture at the start time
start_tspan = np.array([0., 0.])
posture_constraint = ik.PostureConstraint(rbt, start_tspan)
posture_constraint.setJointLimits(free_config_inds,
q0[free_config_inds] - 0.01,
q0[free_config_inds] + 0.01)
constraints.append(posture_constraint)
# Constrain the ee frame to lie on the target point
# facing in the target orientation in between the
# reach and final times)
for t, pose in zip(ee_times, ee_poses):
this_tspan = np.array([t - 0.01, t + 0.01])
constraints.append(
ik.WorldPositionConstraint(rbt,
ee_body,
ee_point,
pose[0:3] - 0.01,
pose[0:3] + 0.01,
tspan=this_tspan))
constraints.append(
ik.WorldEulerConstraint(rbt,
ee_body,
pose[3:6] - 0.05,
pose[3:6] + 0.05,
tspan=this_tspan))
# Seed and nom are both the initial repeated for the #
# of knot points
q_seed = np.hstack([traj_seed.value(t) for t in ts])
q_nom = np.tile(q0, [ts.shape[0], 1]).T
options = ik.IKoptions(rbt)
# Set bounds on initial and final velocities
zero_velocity = np.zeros(rbt.get_num_velocities())
options.setqd0(zero_velocity, zero_velocity)
options.setqdf(zero_velocity, zero_velocity)
Q = np.eye(q0.shape[0], q0.shape[0])
Q[free_config_inds[-2:], free_config_inds[-2:]] *= 1
options.setQ(Q)
options.setQv(np.eye(q0.shape[0], q0.shape[0]) * 10)
results = ik.InverseKinTraj(rbt, ts, q_nom, q_seed, constraints, options)
qtraj = PiecewisePolynomial.Pchip(ts + start_time,
np.vstack(results.q_sol).T, True)
print "IK returned a solution with info %d" % results.info[0]
return qtraj, results.info[0], results.q_sol
