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 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 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.inverseKinSimple(r, q_seed, q_nom, [posture_constraint],
options)
self.assertAlmostEqual(results.q_sol[6], q, 1e-9)
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.inverseKinSimple(r,
q_seed,
q_nom,
[posture_constraint],
options)
self.assertAlmostEqual(results.q_sol[6], q, 1e-9)
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.inverseKinSimple(robot, q_seed, q_seed, constraints, options)
# 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 == 1
print repr(results.q_sol)
ik.WorldPositionConstraint(robot, base_body_id, np.array([0.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([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.inverseKinSimple(robot, q_seed, q_seed, constraints, options)
# 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 == 1
print repr(results.q_sol)
def solveAndDraw(constraints):
results = ik.inverseKinSimple(robot, q_seed, q_seed, constraints,
options)
pose = results.q_sol
robotSystem.robotStateJointController.setPose('pose_end', pose)
def solveAndDraw(constraints):
results = ik.inverseKinSimple(robot, q_seed, q_seed, constraints, options)
pose = results.q_sol
robotSystem.robotStateJointController.setPose('pose_end', pose)