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