def testWorldGazeTargetConstraint(self):
model = self.r
body = 2
axis = np.ones([3, 1])
target = np.ones([3, 1])
gaze_origin = np.zeros([3, 1])
cone_threshold = 1e-3
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.WorldGazeTargetConstraint(model, body, axis, target, gaze_origin,
cone_threshold)
# Test that construction doesn't fail with a given timespan.
ik.WorldGazeTargetConstraint(model, body, axis, target, gaze_origin,
cone_threshold, tspan)
def handleWorldGazeTargetConstraint(self, c, fields):
bodyId = self.bodyNameToId[c.linkName]
tspan = np.asarray(c.tspan, dtype=float)
bodyAxis = np.asarray(c.axis, dtype=float)
gazeOrigin = np.asarray(c.bodyPoint, dtype=float)
targetPosition = np.asarray(c.worldPoint, dtype=float)
coneThreshold = c.coneThreshold
wc = pydrakeik.WorldGazeTargetConstraint(self.rigidBodyTree, bodyId,
bodyAxis, targetPosition,
gazeOrigin, coneThreshold,
tspan)
return wc
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 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]