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
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
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