Esempio n. 1
0
    def IK(self, arm, p, rot, q_guess=None):
        p_kdl = ku.np_vec_to_kdl(p)
        rot_kdl = ku.np_rot_to_kdl(rot)
        fr = kdl.Frame(rot_kdl, p_kdl)

        if q_guess == None:
            if arm == "left_arm":
                q_guess = cgd.find_good_config(p, self.q_guess_dict_left)
            elif arm == "right_arm":
                q_guess = cgd.find_good_config(p, self.q_guess_dict_right)

        q_guess = self.meka_angles_to_kdl(arm, q_guess)

        q_res = self.IK_kdl(arm, fr, q_guess)
        q_res = self.kdl_angles_to_meka(arm, q_res)

        if self.within_joint_limits(arm, q_res):
            if arm == "right_arm":
                if q_res[1] < 0.0:
                    q_res[1] = math.radians(10.0)
                    qg = self.meka_angles_to_kdl(arm, q_res)
                    q_res = self.IK_kdl(arm, fr, qg)
                    q_res = self.kdl_angles_to_meka(arm, q_res)
                if self.within_joint_limits(arm, q_res):
                    return q_res
                else:
                    return None
            else:
                return q_res
        else:
            return None
Esempio n. 2
0
    def IK(self,arm,p,rot,q_guess=None):
        p_kdl = ku.np_vec_to_kdl(p)
        rot_kdl = ku.np_rot_to_kdl(rot)
        fr = kdl.Frame(rot_kdl,p_kdl)

        if q_guess == None:
            if arm == 'left_arm':
                q_guess = cgd.find_good_config(p,self.q_guess_dict_left)
            elif arm == 'right_arm':    
                q_guess = cgd.find_good_config(p,self.q_guess_dict_right)

        q_guess = self.meka_angles_to_kdl(arm,q_guess)

        q_res = self.IK_kdl(arm,fr,q_guess)
        q_res = self.kdl_angles_to_meka(arm,q_res)

        if self.within_joint_limits(arm,q_res):
            if arm == 'right_arm':  
                if q_res[1]<0.:
                    q_res[1] = math.radians(10.)
                    qg = self.meka_angles_to_kdl(arm,q_res)
                    q_res = self.IK_kdl(arm,fr,qg)
                    q_res = self.kdl_angles_to_meka(arm,q_res)
                if self.within_joint_limits(arm,q_res):
                    return q_res
                else:
                    return None
            else:
                return q_res
        else:
            return None
Esempio n. 3
0
    def IK_vanilla(self, p, rot, q_guess=None):
        p_kdl = ku.np_vec_to_kdl(p)
        rot_kdl = ku.np_rot_to_kdl(rot)
        fr = kdl.Frame(rot_kdl, p_kdl)

        if q_guess == None:
            q_guess = cgd.find_good_config(p, self.q_guess_dict)

        q_guess = self.meka_angles_to_kdl(q_guess)
        q_res = self.IK_kdl(arm, fr, q_guess)
        q_res = self.kdl_angles_to_meka(arm, q_res)
        return q_res
Esempio n. 4
0
    def IK_vanilla(self, p, rot, q_guess=None):
        p_kdl = ku.np_vec_to_kdl(p)
        rot_kdl = ku.np_rot_to_kdl(rot)
        fr = kdl.Frame(rot_kdl, p_kdl)

        if q_guess == None:
            q_guess = cgd.find_good_config(p, self.q_guess_dict)

        q_guess = self.meka_angles_to_kdl(q_guess)
        q_res = self.IK_kdl(arm, fr, q_guess)
        q_res = self.kdl_angles_to_meka(arm, q_res)
        return q_res