def testRelativePositionConstraint(self):
model = self.r
pts = np.zeros([3, 1])
lb = -np.ones([3, 1])
ub = np.ones([3, 1])
bodyA_idx = 1
bodyB_idx = 2
translation = np.zeros(3)
quaternion = np.array([1, 0, 0, 0])
bTbp = np.concatenate([translation, quaternion]).reshape(7, 1)
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.RelativePositionConstraint(model, pts, lb, ub, bodyA_idx, bodyB_idx,
bTbp)
# Test that construction doesn't fail with a given timespan.
ik.RelativePositionConstraint(model, pts, lb, ub, bodyA_idx, bodyB_idx,
bTbp, tspan)
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]