コード例 #1
0
ファイル: Bone.py プロジェクト: Ecialo/Codename-Parzalon
class Bone(object):

    def __init__(self, name=None, parent=None, length=0, x=0, y=0, scaleX=1, scaleY=1, rotation=0):

        self.bone_data = None

        self.name = name
        self.local_tsr = TSR((x, y), scaleX, scaleY, rotation)
        self.global_tsr = TSR()
        self.parent = parent
        self.length = length
        self.childs = []

    def update_childs(self):
        data = self.global_tsr
        reduce(lambda _, child: child.update(data), self.childs, None)

    def update(self, data):
        #self.position, self.scale, self.rotation = data
        #pos, scale_x, scale_y, rot = self.bone_data[3:7:]
        #par_pos, par_scale_x, par_scale_y, par_rot = data
        #self.position, self.scale_x, self.scale_y, self.rotation = tsr_transform(data, self.bone_data[3:7:])
        self.global_tsr.set_by_named_pack(self.local_tsr.tsr_transform(data))
        self.update_childs()

    def add_bone(self, bone):
        self.childs.append(bone)

    def apply_bone_data(self):
        self.bone_data = Bone_Data(self.name, self.parent, self.length, self.local_tsr.copy())
コード例 #2
0
def supportTorsoPose(supports):
    #Calculate torso pose based on grip manips (i.e. best guess based on some parameters
    # Update the passed in reference with the Torso Manip
    t = mat([0, 0, 0]).T
    #Define ideal vertical offsets from each end effector
    numS = len(supports)
    L = {
        'leftArmManip': .1,
        'rightArmManip': .1,
        'leftFootManip': .75,
        'rightFootManip': .75
    }
    w = {
        'leftArmManip': 1,
        'rightArmManip': 1,
        'leftFootManip': 5,
        'rightFootManip': 5
    }
    wsum = 0
    for s in supports.keys():
        #TODO: remove expensive string comparison? maybe pre-subtract the affine manip
        #TODO: use centroid calculation instead of mean?
        if s == 'affineManip':
            continue
        T = supports[s].endPose()
        #print T[:3,-1]
        t = t + (T[:3, -1] + mat([-.1, 0, L[s]]).T) * w[s]
        wsum += w[s]

    affineTSR = TSR(MakeTransform(mat(eye(3)), t / wsum))
    affineTSR.Bw = mat(
        [-.05, .05, -.05, .05, -.1, .15, 0, 0, 0, 0, -pi / 16, pi / 16])
    affineTSR.manipindex = 4
    return affineTSR
コード例 #3
0
ファイル: Bone.py プロジェクト: Ecialo/Codename-Parzalon
    def __init__(self, name=None, parent=None, length=0, x=0, y=0, scaleX=1, scaleY=1, rotation=0):

        self.bone_data = None

        self.name = name
        self.local_tsr = TSR((x, y), scaleX, scaleY, rotation)
        self.global_tsr = TSR()
        self.parent = parent
        self.length = length
        self.childs = []
コード例 #4
0
ファイル: planning.py プロジェクト: paresh471/openHubo
def supportTorsoPose(supports):
    # Calculate torso pose based on grip manips (i.e. best guess based on some parameters
    # Update the passed in reference with the Torso Manip
    t = mat([0, 0, 0]).T
    # Define ideal vertical offsets from each end effector
    numS = len(supports)
    L = {"leftArmManip": 0.1, "rightArmManip": 0.1, "leftFootManip": 0.75, "rightFootManip": 0.75}
    w = {"leftArmManip": 1, "rightArmManip": 1, "leftFootManip": 5, "rightFootManip": 5}
    wsum = 0
    for s in supports.keys():
        # TODO: remove expensive string comparison? maybe pre-subtract the affine manip
        # TODO: use centroid calculation instead of mean?
        if s == "affineManip":
            continue
        T = supports[s].endPose()
        # print T[:3,-1]
        t = t + (T[:3, -1] + mat([-0.1, 0, L[s]]).T) * w[s]
        wsum += w[s]

    affineTSR = TSR(MakeTransform(mat(eye(3)), t / wsum))
    affineTSR.Bw = mat([-0.05, 0.05, -0.05, 0.05, -0.1, 0.15, 0, 0, 0, 0, -pi / 16, pi / 16])
    affineTSR.manipindex = 4
    return affineTSR
コード例 #5
0
def MakeInPlaceConstraint(robot, manipname):
    manips = robot.GetManipulators()
    manip = robot.GetManipulator(manipname)
    link = manip.GetEndEffector()
    T0_w = mat(link.GetTransform())
    Tw_e = mat(eye(4))
    for i in range(len(manips)):
        if manips[i].GetName() == manipname:
            manipindex = i
            break
    print manipindex

    tsr = TSR(T0_w, Tw_e, mat(zeros(12)), manipindex)
    print tsr.manipindex
    chain = TSRChain(0, 1, 1)
    chain.insertTSR(tsr)
    return chain
コード例 #6
0
    setInitialPose(robot)
    time.sleep(1)
    
    #Define manips used and goals
    z1=.01
    theta=0.1
    LH=0    
    RH=8
    POST=8
    RUNG0=16
    RUNG=2
    LF=0
    RF=1

    #Post grips at shoulder height
    rgrip1=TSR(grips[3+RH],MakeTransform(eye(3),matrix([.0,-.015,0]).T),mat([0,0, 0,0, -z1,z1, 0,0 ,0,0, -theta,theta]),1)
    lgrip1=TSR(grips[3],MakeTransform(eye(3),matrix([.0,.015,0]).T),mat([0,0, 0,0, -z1,z1, 0,0 ,0,0, -theta,theta]),0)
    
    #Rung grips at shoulder height
    rgrip2=TSR(grips[RUNG0+2*RUNG+RH],MakeTransform(eye(3),matrix([.0,-.015,0]).T),mat([0,0, 0,0, -z1,z1, 0,0 ,0,0, -theta,theta]),1)
    lgrip2=TSR(grips[RUNG0+2*RUNG],MakeTransform(eye(3),matrix([.0,.015,0]).T),mat([0,0, 0,0, -z1,z1, 0,0 ,0,0, -theta,theta]),0)
    
    #Step onto first rung
    lstep1=TSR(grips[RUNG0+LF],MakeTransform(eye(3),matrix([.0,0,.014]).T),mat([0,0, 0,0, 0,0, 0,0 ,-pi/3,0, 0,0]),2)
    rstep1=TSR(grips[RUNG0+RF],MakeTransform(eye(3),matrix([.0,0,0.014]).T),mat([0,0, 0,0, 0,0, 0,0 ,-theta,theta, 0,0]),3)

    # Define keyframe poses in terms of manips
    pose1={'rightArm':rgrip1,'leftArm':lgrip1,'leftFoot':lstep1}
    pose2={'rightArm':rgrip2,'leftArm':lgrip2,'leftFoot':lstep1}

    print "Place the robot in the desired starting position"
コード例 #7
0
        return self.solved()
    
    def continousSolve(self,itrs=1000,auto=False,extra=[]):
        #Enable TSR sampling
        self.sample_bw=True
        for k in range(itrs):
            if not(self.robot.GetEnv().CheckCollision(self.robot)) and not(self.robot.CheckSelfCollision()) and self.solved():
                print "Valid solution found!"
                #TODO: log solutuon + affine DOF

            #rezero to prevent getting stuck...at major speed penalty
            self.robot.SetDOFValues(self.zero)
            self.run(auto,extra)
            time.sleep(.1)
        return self.solved()
    
    def resetZero(self):
        self.zero=self.robot.GetDOFValues()
    
if __name__ == '__main__':
    
    juiceTSR = TSR()
    juiceTSR.Tw_e = MakeTransform(rodrigues([pi/2, 0, 0]),mat([0, 0.22, 0.1]).T)
    juiceTSR.Bw = mat([0, 0,   0, 0,   -0.02, 0.02,   0, 0,   0, 0,   -pi, pi])
    juiceTSR.manipindex = 0
    
    test=GeneralIK('','',[juiceTSR],False)
    test.supportlinks=['leftFootBase']
    test.cogtarget=(.1,.2,.3)
    print test.Serialize()