def handleFixedLinkFromRobotPoseConstraint(self, c, fields):
bodyId = self.bodyNameToId[c.linkName]
pose = np.asarray(fields.poses[c.poseName], dtype=float)
pointInBodyFrame = np.zeros(3)
tolerance = np.radians(c.angleToleranceInDegrees)
tspan = np.asarray(c.tspan, dtype=float)
bodyToWorld = self.computeBodyToWorld(pose, c.linkName,
pointInBodyFrame)
bodyPos = transformUtils.transformations.translation_from_matrix(
bodyToWorld)
bodyQuat = transformUtils.transformations.quaternion_from_matrix(
bodyToWorld)
lowerBound = bodyPos + c.lowerBound
upperBound = bodyPos + c.upperBound
pc = pydrakeik.WorldPositionConstraint(self.rigidBodyTree, bodyId,
pointInBodyFrame, lowerBound,
upperBound, tspan)
qc = pydrakeik.WorldQuatConstraint(self.rigidBodyTree, bodyId,
bodyQuat, tolerance, tspan)
return [pc, qc]
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 onFrameModified(obj):
pos, quat = transformUtils.poseFromTransform(obj.transform)
pc = ik.WorldPositionConstraint(robot, leftHandBodyId, np.zeros((3, )),
pos, pos, tspan)
qc = ik.WorldQuatConstraint(robot, leftHandBodyId, quat, 0.0, tspan)
solveAndDraw([qsc, pc, qc] + footConstraints)
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 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 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
def testIkPlan():
ikPlanner = robotSystem.ikPlanner
constraints = buildConstraints()
poses = ce.getPlanPoses(constraints, ikPlanner)
global robot
robot = loadRigidBodyTree(getIkUrdfFilename())
tspan = np.array([1.0, 1.0])
handLinkNames = [ikPlanner.getHandLink('left'), ikPlanner.getHandLink('right')]
footLinkNames = [robotSystem.directorConfig['leftFootLink'], robotSystem.directorConfig['rightFootLink']]
leftHandBodyId = robot.FindBody(str(handLinkNames[0])).get_body_index()
rightHandBodyId = robot.FindBody(str(handLinkNames[1])).get_body_index()
leftFootBodyId = robot.FindBody(str(footLinkNames[0])).get_body_index()
rightFootBodyId = robot.FindBody(str(footLinkNames[1])).get_body_index()
lfootContactPts, rfootContactPts = getFootContactPoints()
#lfootContactPts, rfootContactPts = = robotSystem.footstepsDriver.getContactPts()
for i in xrange(robot.get_num_bodies()):
body = robot.get_body(i)
print i, body.get_name()
assert robot.FindBodyIndex(body.get_name()) == i
q = np.zeros(robot.get_num_positions())
v = np.zeros(robot.get_num_velocities())
kinsol = robot.doKinematics(q,v)
t = robot.relativeTransform(kinsol, robot.FindBodyIndex('world'), robot.FindBodyIndex(str(footLinkNames[0])))
tt = transformUtils.getTransformFromNumpy(t)
pos = transformUtils.transformations.translation_from_matrix(t)
quat = transformUtils.transformations.quaternion_from_matrix(t)
footConstraints = []
for linkName in footLinkNames:
t = robotSystem.robotStateModel.getLinkFrame(linkName)
pos, quat = transformUtils.poseFromTransform(t)
if False and linkName == footLinkNames[0]:
pc = pydrakeik.WorldPositionConstraint(robot, robot.findLink(str(linkName)).get_body_index(), np.zeros((3,)), pos + [-10,-10,0], pos+[10,10,0], tspan)
else:
pc = pydrakeik.WorldPositionConstraint(robot, robot.findLink(str(linkName)).get_body_index(), np.zeros((3,)), pos, pos, tspan)
qc = pydrakeik.WorldQuatConstraint(robot, robot.findLink(str(linkName)).get_body_index(), quat, 0.01, tspan)
footConstraints.append(pc)
footConstraints.append(qc)
qsc = pydrakeik.QuasiStaticConstraint(robot, tspan)
qsc.setActive(True)
qsc.setShrinkFactor(1.0)
qsc.addContact([leftFootBodyId], lfootContactPts.transpose())
qsc.addContact([rightFootBodyId], rfootContactPts.transpose())
#q_seed = robot.getZeroConfiguration()
#q_seed = robotSystem.robotStateJointController.getPose('q_nom').copy()
# todo, joint costs, and other options
global options
options = pydrakeik.IKoptions(robot)
jointIndexMap = np.zeros(robotSystem.robotStateJointController.numberOfJoints)
drakePositionNames = [robot.get_position_name(i) for i in xrange(robot.get_num_positions())]
for i, name in enumerate(robotSystem.robotStateJointController.jointNames):
jointIndexMap[i] = drakePositionNames.index(name)
jointIndexMap = list(jointIndexMap)
print jointIndexMap
q_seed = np.zeros(robot.get_num_positions())
q_seed[jointIndexMap] = robotSystem.robotStateJointController.getPose('q_nom').copy()
# setup costs
costs = np.ones(robot.get_num_positions())
groupCosts = [
('Left Leg', 1e3),
('Right Leg', 1e3),
('Left Arm', 1),
('Right Arm', 1),
('Back', 1e4),
('Neck', 1e6),
]
for groupName, costValue in groupCosts:
names = robotSystem.ikPlanner.getJointGroup(groupName)
inds = [drakePositionNames.index(name) for name in names]
print costValue, names
costs[inds] = costValue
costs[drakePositionNames.index('base_x')] = 0
costs[drakePositionNames.index('base_y')] = 0
costs[drakePositionNames.index('base_z')] = 0
costs[drakePositionNames.index('base_roll')] = 1e3
costs[drakePositionNames.index('base_pitch')] = 1e3
costs[drakePositionNames.index('base_yaw')] = 0
print costs
options.setQ(np.diag(costs))
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 onFrameModified(obj):
pos, quat = transformUtils.poseFromTransform(obj.transform)
pc = pydrakeik.WorldPositionConstraint(robot, leftHandBodyId,
np.zeros((3,)),
pos,
pos, tspan)
qc = pydrakeik.WorldQuatConstraint(robot, leftHandBodyId,
quat, 0.01, tspan)
solveAndDraw([qsc, pc, qc] + footConstraints)
#solveAndDraw([pc])
frameObj = vis.updateFrame(robotSystem.robotStateModel.getLinkFrame(ikPlanner.getHandLink('left')), 'left hand frame')
frameObj.setProperty('Edit', True)
frameObj.setProperty('Scale', 0.2)
frameObj.connectFrameModified(onFrameModified)
hand_frame_id = robot.findFrame("r_hand_frame").get_frame_index()
base_body_id = robot.FindBody('base_footprint').get_body_index()
constraints = [
# These three constraints ensure that the base of the robot is
# at z = 0 and has no pitch or roll. Instead of directly
# constraining orientation, we just require that the points at
# [0, 0, 0], [1, 0, 0], and [0, 1, 0] in the robot's base's
# frame must all be at z = 0 in world frame.
# We don't care about the x or y position of the robot's base,
# so we use NaN values to tell the IK solver not to apply a
# constraint along those dimensions. This is equivalent to
# placing a lower bound of -Inf and an upper bound of +Inf along
# those axes.
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]),
def plan_grasping_trajectory(rbt, q0, target_reach_pose, target_grasp_pose,
n_knots, reach_time, grasp_time):
''' Solves IK at a series of sample times (connected with a
cubic spline) to generate a trajectory to bring the Kuka from an
initial pose q0 to a final end effector pose in the specified
time, using the specified number of knot points.
Uses an intermediate pose reach_pose as an intermediate target
to hit at the knot point closest to reach_time.
See http://drake.mit.edu/doxygen_cxx/rigid__body__ik_8h.html
for the "inverseKinTraj" entry. At the moment, the Python binding
for this function uses "inverseKinTrajSimple" -- i.e., it doesn't
return derivatives. '''
nq = rbt.get_num_positions()
q_des_full = np.zeros(nq)
# Create knot points
ts = np.linspace(0., grasp_time, n_knots)
# Figure out the knot just before reach time
reach_start_index = np.argmax(ts >= reach_time) - 1
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 for all time
all_tspan = np.array([0., grasp_time])
posture_constraint = ik.PostureConstraint(rbt, all_tspan)
posture_constraint.setJointLimits(constrained_config_inds,
q0[constrained_config_inds] - 0.01,
q0[constrained_config_inds] + 0.01)
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
ee_frame = rbt.findFrame("iiwa_frame_ee").get_frame_index()
for i in range(reach_start_index, n_knots):
this_tspan = np.array([ts[i], ts[i]])
interp = float(i - reach_start_index) / (n_knots - reach_start_index)
target_pose = (1. -
interp) * target_reach_pose + interp * target_grasp_pose
constraints.append(
ik.WorldPositionConstraint(rbt,
ee_frame,
np.zeros((3, 1)),
target_pose[0:3] - 0.01,
target_pose[0:3] + 0.01,
tspan=this_tspan))
constraints.append(
ik.WorldEulerConstraint(rbt,
ee_frame,
target_pose[3:6] - 0.05,
target_pose[3:6] + 0.05,
tspan=this_tspan))
# Seed and nom are both the initial repeated for the #
# of knot points
q_seed = np.tile(q0, [1, n_knots])
q_nom = np.tile(q0, [1, n_knots])
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)
results = ik.InverseKinTraj(rbt, ts, q_seed, q_nom, constraints, options)
qtraj = PiecewisePolynomial.Pchip(ts, np.vstack(results.q_sol).T, True)
print "IK returned a solution with info %d" % results.info[0]
print "(Info 1 is good, other values are dangerous)"
return qtraj, results.info[0]
import os
import numpy as np
import pydrake
from pydrake.solvers import ik
robot = pydrake.rbtree.RigidBodyTree(
os.path.join(pydrake.getDrakePath(), "examples/PR2/pr2.urdf"))
# Make sure attribute access works on bodies
assert robot.bodies[0].linkname == "world"
constraints = [
ik.WorldPositionConstraint(robot, 1, np.zeros((3, )), np.array([0, 0,
1.0]),
np.array([0, 0, 1.0])),
ik.WorldPositionConstraint(
robot,
robot.findLink("r_gripper_palm_link").body_index, np.zeros((3, )),
np.array([0.5, 0, 1.5]), np.array([0.5, 0, 1.5])),
]
q_seed = robot.getZeroConfiguration()
options = ik.IKoptions(robot)
results = ik.inverseKinSimple(robot, q_seed, q_seed, constraints, options)
print repr(results.q_sol)
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
def testIkPlan():
ikPlanner = robotSystem.ikPlanner
ikPlanner.pushToMatlab = False
constraints = buildConstraints()
poses = ce.getPlanPoses(constraints, ikPlanner)
robot = loadRigidBodyTree(getIkUrdfFilename())
#for i, body in enumerate(robot.bodies):
# print i, body.linkname
tspan = np.array([1.0, 1.0])
handLinkNames = [
ikPlanner.getHandLink('left'),
ikPlanner.getHandLink('right')
]
footLinkNames = [
robotSystem.directorConfig['leftFootLink'],
robotSystem.directorConfig['rightFootLink']
]
leftHandBodyId = robot.findLink(str(handLinkNames[0])).body_index
rightHandBodyId = robot.findLink(str(handLinkNames[1])).body_index
leftFootBodyId = robot.findLink(str(footLinkNames[0])).body_index
rightFootBodyId = robot.findLink(str(footLinkNames[1])).body_index
lfootContactPts, rfootContactPts = robotSystem.footstepsDriver.getContactPts(
)
footConstraints = []
for linkName in footLinkNames:
t = robotSystem.robotStateModel.getLinkFrame(linkName)
pos, quat = transformUtils.poseFromTransform(t)
pc = ik.WorldPositionConstraint(
robot,
robot.findLink(str(linkName)).body_index, np.zeros((3, )), pos,
pos, tspan)
qc = ik.WorldQuatConstraint(robot,
robot.findLink(str(linkName)).body_index,
quat, 0.0, tspan)
footConstraints.append(pc)
footConstraints.append(qc)
qsc = ik.QuasiStaticConstraint(robot, tspan)
qsc.setActive(True)
qsc.setShrinkFactor(0.5)
qsc.addContact([leftFootBodyId], lfootContactPts.transpose())
qsc.addContact([rightFootBodyId], rfootContactPts.transpose())
#q_seed = robot.getZeroConfiguration()
q_seed = robotSystem.robotStateJointController.getPose('q_nom').copy()
# todo, joint costs, and other options
options = ik.IKoptions(robot)
def solveAndDraw(constraints):
results = ik.inverseKinSimple(robot, q_seed, q_seed, constraints,
options)
pose = results.q_sol
robotSystem.robotStateJointController.setPose('pose_end', pose)
def onFrameModified(obj):
pos, quat = transformUtils.poseFromTransform(obj.transform)
pc = ik.WorldPositionConstraint(robot, leftHandBodyId, np.zeros((3, )),
pos, pos, tspan)
qc = ik.WorldQuatConstraint(robot, leftHandBodyId, quat, 0.0, tspan)
solveAndDraw([qsc, pc, qc] + footConstraints)
frameObj = vis.updateFrame(
robotSystem.robotStateModel.getLinkFrame(
ikPlanner.getHandLink('left')), 'left hand frame')
frameObj.setProperty('Edit', True)
frameObj.setProperty('Scale', 0.2)
frameObj.connectFrameModified(onFrameModified)
