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)