def get_kinematics_config(robot, joint_id, link_id, open_chain_joints, base_link, ee_link): joint_screws_in_ee = np.zeros((6, len(open_chain_joints))) ee_link_state = p.getLinkState(robot, link_id[ee_link], 1, 1) if link_id[base_link] == -1: base_pos, base_quat = p.getBasePositionAndOrientation(robot) else: base_link_state = p.getLinkState(robot, link_id[base_link], 1, 1) base_pos, base_quat = base_link_state[0], base_link_state[1] T_w_b = liegroup.RpToTrans(util.quat_to_rot(np.array(base_quat)), np.array(base_pos)) T_w_ee = liegroup.RpToTrans(util.quat_to_rot(np.array(ee_link_state[1])), np.array(ee_link_state[0])) T_b_ee = np.dot(liegroup.TransInv(T_w_b), T_w_ee) for i, joint_name in enumerate(open_chain_joints): joint_info = p.getJointInfo(robot, joint_id[joint_name]) link_name = joint_info[12].decode("utf-8") joint_type = joint_info[2] joint_axis = joint_info[13] screw_at_joint = np.zeros(6) link_state = p.getLinkState(robot, link_id[link_name], 1, 1) T_w_j = liegroup.RpToTrans(util.quat_to_rot(np.array(link_state[5])), np.array(link_state[4])) T_ee_j = np.dot(liegroup.TransInv(T_w_ee), T_w_j) Adj_ee_j = liegroup.Adjoint(T_ee_j) if joint_type == p.JOINT_REVOLUTE: screw_at_joint[0:3] = np.array(joint_axis) elif joint_type == p.JOINT_PRISMATIC: screw_at_joint[3:6] = np.array(joint_axis) else: raise ValueError joint_screws_in_ee[:, i] = np.dot(Adj_ee_j, screw_at_joint) return joint_screws_in_ee, T_b_ee
def ik_feet(base_pos, base_quat, lf_pos, lf_quat, rf_pos, rf_quat, nominal_sensor_data, joint_screws_in_ee_at_home, ee_SE3_at_home, open_chain_joints): joint_pos = copy.deepcopy(nominal_sensor_data['joint_pos']) T_w_base = liegroup.RpToTrans(util.quat_to_rot(base_quat), base_pos) # left foot lf_q_guess = np.array([ nominal_sensor_data['joint_pos'][j_name] for j_name in open_chain_joints[0] ]) T_w_lf = liegroup.RpToTrans(util.quat_to_rot(lf_quat), lf_pos) T_base_lf = np.dot(liegroup.TransInv(T_w_base), T_w_lf) lf_q_sol, lf_done = robot_kinematics.IKinBody( joint_screws_in_ee_at_home[0], ee_SE3_at_home[0], T_base_lf, lf_q_guess) for j_id, j_name in enumerate(open_chain_joints[0]): joint_pos[j_name] = lf_q_sol[j_id] # right foot rf_q_guess = np.array([ nominal_sensor_data['joint_pos'][j_name] for j_name in open_chain_joints[1] ]) T_w_rf = liegroup.RpToTrans(util.quat_to_rot(rf_quat), rf_pos) T_base_rf = np.dot(liegroup.TransInv(T_w_base), T_w_rf) rf_q_sol, rf_done = robot_kinematics.IKinBody( joint_screws_in_ee_at_home[1], ee_SE3_at_home[1], T_base_rf, rf_q_guess) for j_id, j_name in enumerate(open_chain_joints[1]): joint_pos[j_name] = rf_q_sol[j_id] return joint_pos, lf_done, rf_done
# Solve Inverse Kinematics base_pos = np.copy(vis_base_lin_pos) base_quat = np.copy( util.rot_to_quat(util.euler_to_rot(vis_base_ang_pos))) for ee in range(2): q_guess = np.array([ nominal_sensor_data['joint_pos'][j_name] for j_name in open_chain_joints[ee] ]) T_w_base = liegroup.RpToTrans(util.euler_to_rot(vis_base_ang_pos), vis_base_lin_pos) T_w_ee = liegroup.RpToTrans( util.euler_to_rot(vis_ee_motion_ang_pos[ee]), vis_ee_motion_lin_pos[ee]) des_T = np.dot(liegroup.TransInv(T_w_base), T_w_ee) # << T_base_ee q_sol, done = robot_kinematics.IKinBody( joint_screws_in_ee_at_home[ee], ee_SE3_at_home[ee], des_T, q_guess) for j_id, j_name in enumerate(open_chain_joints[ee]): joint_pos[j_name] = q_sol[j_id] if not done: print("====================================") print("Sovling inverse kinematics for ee-{} at time {}".format( ee, vis_t)) print("success: {}".format(done)) print("T_w_base") print(T_w_base) print("T_w_ee") print(T_w_ee)