Esempio n. 1
0
    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)
Esempio n. 3
0
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)
Esempio n. 4
0
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)
Esempio n. 5
0
 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
Esempio n. 6
0
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)
Esempio n. 7
0
    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()
Esempio n. 10
0
 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
Esempio n. 11
0
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)
Esempio n. 12
0
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)
Esempio n. 13
0
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)