def setupFields(self,
constraints,
ikParameters,
positionCosts,
nominalPoseName="",
seedPoseName="",
endPoseName=""):
poses = ikconstraintencoder.getPlanPoses(constraints, self.ikPlanner)
poses.update(self.poses)
poses['q_nom'] = list(self.ikPlanner.jointController.getPose('q_nom'))
fields = FieldContainer(
utime=getUtime(),
poses=poses,
constraints=constraints,
seedPose=seedPoseName,
nominalPose=nominalPoseName,
endPose=endPoseName,
jointNames=self.jointNames,
jointLimits=self.jointLimits,
positionCosts=positionCosts,
affordances=self.processAffordances(),
options=ikParameters,
)
return fields
def testPlanConstraints():
ikPlanner.planningMode = 'dummy'
constraints = buildConstraints()
poses = ce.getPlanPoses(constraints, ikPlanner)
poseJsonStr = json.dumps(poses, indent=4)
constraintsJsonStr = ce.encodeConstraints(constraints, indent=4)
print poseJsonStr
print constraintsJsonStr
constraints = ce.decodeConstraints(constraintsJsonStr)
pprint.pprint(constraints)
def testPlanConstraints():
# this is required for now, makes it avoid communication with matlab
# inside the call to ikPlanner.addPose
ikPlanner.pushToMatlab = False
constraints = buildConstraints()
poses = ce.getPlanPoses(constraints, ikPlanner)
poseJsonStr = json.dumps(poses, indent=4)
constraintsJsonStr = ce.encodeConstraints(constraints, indent=4)
print poseJsonStr
print constraintsJsonStr
constraints = ce.decodeConstraints(constraintsJsonStr)
pprint.pprint(constraints)
def testPlanConstraints():
ikPlanner.planningMode = 'dummy'
constraints = buildConstraints()
poses = ce.getPlanPoses(constraints, ikPlanner)
poseJsonStr = json.dumps(poses, indent=4)
constraintsJsonStr = ce.encodeConstraints(constraints, indent=4)
print poseJsonStr
print constraintsJsonStr
constraints = ce.decodeConstraints(constraintsJsonStr)
pprint.pprint(constraints)
def setupMessage(self, constraints, endPoseName="", nominalPoseName="", seedPoseName="", additionalTimeSamples=None):
poses = ikconstraintencoder.getPlanPoses(constraints, self.ikPlanner)
poses.update(self.poses)
msg = lcmdrc.exotica_planner_request_t()
msg.utime = getUtime()
msg.poses = json.dumps(poses)
msg.constraints = ikconstraintencoder.encodeConstraints(constraints)
msg.seed_pose = seedPoseName
msg.nominal_pose = nominalPoseName
msg.end_pose = endPoseName
msg.joint_names = json.dumps(list(self.ikPlanner.jointController.jointNames))
msg.affordances = self.processAffordances()
opt=ikplanner.getIkOptions()._properties
if additionalTimeSamples:
opt.update({'timeSamples':additionalTimeSamples})
opt['jointLimits']=self.jointLimits
msg.options = json.dumps(opt)
return msg
def testPlanConstraints():
# this is required for now, makes it avoid communication with matlab
# inside the call to ikPlanner.addPose
ikPlanner.pushToMatlab = False
constraints = buildConstraints()
poses = ce.getPlanPoses(constraints, ikPlanner)
poseJsonStr = json.dumps(poses, indent=4)
constraintsJsonStr = ce.encodeConstraints(constraints, indent=4)
print poseJsonStr
print constraintsJsonStr
constraints = ce.decodeConstraints(constraintsJsonStr)
pprint.pprint(constraints)
def setupFields(self, constraints, ikParameters, positionCosts, nominalPoseName="", seedPoseName="", endPoseName=""):
poses = ikconstraintencoder.getPlanPoses(constraints, self.ikPlanner)
poses.update(self.poses)
poses['q_nom'] = list(self.ikPlanner.jointController.getPose('q_nom'))
fields = FieldContainer(
utime = getUtime(),
poses = poses,
constraints = constraints,
seedPose = seedPoseName,
nominalPose = nominalPoseName,
endPose = endPoseName,
jointNames = self.jointNames,
jointLimits = self.jointLimits,
positionCosts = positionCosts,
affordances = self.processAffordances(),
options = ikParameters,
)
return fields
def testPlanConstraints():
ikPlanner.planningMode = 'dummy'
constraints = buildConstraints()
poses = ce.getPlanPoses(constraints, ikPlanner)
poseJsonStr = json.dumps(poses, indent=4)
constraintsJsonStr = ce.encodeConstraints(constraints, indent=4)
print poseJsonStr
print constraintsJsonStr
print '--------------decoding--------------------'
constraints = ce.decodeConstraints(constraintsJsonStr)
pprint.pprint(constraints)
print '--------------reconstructing--------------'
constraints = reconstructConstraints(constraints)
print '--------------matlab commands---------------'
for c in constraints:
c.printCommands()
def testPlanConstraints():
ikPlanner.planningMode = 'dummy'
constraints = buildConstraints()
poses = ce.getPlanPoses(constraints, ikPlanner)
poseJsonStr = json.dumps(poses, indent=4)
constraintsJsonStr = ce.encodeConstraints(constraints, indent=4)
print poseJsonStr
print constraintsJsonStr
print '--------------decoding--------------------'
constraints = ce.decodeConstraints(constraintsJsonStr)
pprint.pprint(constraints)
print '--------------reconstructing--------------'
constraints = reconstructConstraints(constraints)
print '--------------matlab commands---------------'
for c in constraints:
c.printCommands()
def setupMessage(self,
constraints,
endPoseName="",
nominalPoseName="",
seedPoseName="",
additionalTimeSamples=None):
poses = ikconstraintencoder.getPlanPoses(constraints, self.ikPlanner)
poses.update(self.poses)
msg = lcmdrc.exotica_planner_request_t()
msg.utime = getUtime()
msg.poses = json.dumps(poses)
msg.constraints = ikconstraintencoder.encodeConstraints(constraints)
msg.seed_pose = seedPoseName
msg.nominal_pose = nominalPoseName
msg.end_pose = endPoseName
msg.joint_names = json.dumps(
list(self.ikPlanner.jointController.jointNames))
msg.affordances = self.processAffordances()
opt = ikplanner.getIkOptions()._properties
if additionalTimeSamples:
opt.update({'timeSamples': additionalTimeSamples})
opt['jointLimits'] = self.jointLimits
msg.options = json.dumps(opt)
return msg
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)
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)
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)
