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
def iso_interpolate(T1, T2, alpha): p1 = T1[0:3, 3] R1 = T1[0:3, 0:3] p2 = T2[0:3, 3] R2 = T2[0:3, 0:3] slerp = Slerp([0, 1], R.from_matrix([R1, R2])) p_ret = alpha * (p1 + p2) R_ret = slerp(alpha).as_matrix() return liegroup.RpToTrans(R_ret, p_ret)
for ee in range(2): vis_ee_motion_lin_pos[ee] = vis_ee_motion_lin[ee][vis_idx, 0:3] vis_ee_motion_ang_pos[ee] = vis_ee_motion_ang[ee][vis_idx, 0:3] vis_ee_wrench_lin_pos[ee] = vis_ee_wrench_lin[ee][vis_idx, 0:3] vis_ee_wrench_ang_pos[ee] = vis_ee_wrench_ang[ee][vis_idx, 0:3] # 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))
def run_sim(inp): goal_pos = np.copy(inp) angle = 0. if not p.isConnected(): if not B_VISUALIZE: p.connect(p.DIRECT) else: p.connect(p.GUI) p.resetDebugVisualizerCamera(cameraDistance=2.0, cameraYaw=180 + 45, cameraPitch=-15, cameraTargetPosition=[0.5, 0.5, 0.6]) p.resetSimulation() p.setGravity(0, 0, -9.8) p.setPhysicsEngineParameter(fixedTimeStep=SimConfig.CONTROLLER_DT, numSubSteps=SimConfig.N_SUBSTEP) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) robot = p.loadURDF( cwd + "/robot_model/draco3/draco3_gripper_mesh_updated.urdf", SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT, SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT) p.loadURDF(cwd + "/robot_model/ground/plane.urdf", [0, 0, 0]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config( robot, SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT, SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT, False) if B_VISUALIZE: lh_target_frame = p.loadURDF(cwd + "/robot_model/etc/ball.urdf", [0., 0, 0.], [0, 0, 0, 1]) lh_target_pos = np.array([0., 0., 0.]) lh_target_quat = np.array([0., 0., 0., 1.]) lh_waypoint_frame = p.loadURDF(cwd + "/robot_model/etc/ball.urdf", [0., 0, 0.], [0, 0, 0, 1]) lh_waypoint_pos = np.array([0., 0., 0.]) lh_waypoint_quat = np.array([0., 0., 0., 1.]) # Add Gear constraint c = p.createConstraint(robot, link_id['l_knee_fe_lp'], robot, link_id['l_knee_fe_ld'], jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(c, gearRatio=-1, maxForce=500, erp=10) c = p.createConstraint(robot, link_id['r_knee_fe_lp'], robot, link_id['r_knee_fe_ld'], jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, 0]) p.changeConstraint(c, gearRatio=-1, maxForce=500, erp=10) # Initial Config set_initial_config(robot, joint_id) # Link Damping pybullet_util.set_link_damping(robot, link_id.values(), 0., 0.) # Joint Friction pybullet_util.set_joint_friction(robot, joint_id, 0.) gripper_attached_joint_id = OrderedDict() gripper_attached_joint_id["l_wrist_pitch"] = joint_id["l_wrist_pitch"] gripper_attached_joint_id["r_wrist_pitch"] = joint_id["r_wrist_pitch"] pybullet_util.set_joint_friction(robot, gripper_attached_joint_id, 0.1) # Construct Interface interface = DracoManipulationInterface() # Run Sim t = 0 dt = SimConfig.CONTROLLER_DT count = 0 nominal_sensor_data = pybullet_util.get_sensor_data( robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom) if B_VISUALIZE: pybullet_util.draw_link_frame(robot, link_id['l_hand_contact'], text="lh") pybullet_util.draw_link_frame(lh_target_frame, -1, text="lh_target") pybullet_util.draw_link_frame(lh_waypoint_frame, -1) gripper_command = dict() for gripper_joint in GRIPPER_JOINTS: gripper_command[gripper_joint] = nominal_sensor_data['joint_pos'][ gripper_joint] waiting_command = True time_command_recved = 0. while (1): # Get SensorData sensor_data = pybullet_util.get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom) for gripper_joint in GRIPPER_JOINTS: del sensor_data['joint_pos'][gripper_joint] del sensor_data['joint_vel'][gripper_joint] rf_height = pybullet_util.get_link_iso(robot, link_id['r_foot_contact'])[2, 3] lf_height = pybullet_util.get_link_iso(robot, link_id['l_foot_contact'])[2, 3] sensor_data['b_rf_contact'] = True if rf_height <= 0.01 else False sensor_data['b_lf_contact'] = True if lf_height <= 0.01 else False if t >= WalkingConfig.INIT_STAND_DUR + 0.1 and waiting_command: global_goal_pos = np.copy(goal_pos) global_goal_pos[0] += sensor_data['base_com_pos'][0] global_goal_pos[1] += sensor_data['base_com_pos'][1] lh_target_pos = np.copy(global_goal_pos) lh_target_rot = np.dot(RIGHTUP_GRIPPER, x_rot(angle)) lh_target_quat = util.rot_to_quat(lh_target_rot) lh_target_iso = liegroup.RpToTrans(lh_target_rot, lh_target_pos) lh_waypoint_pos = generate_keypoint(lh_target_iso)[0:3, 3] interface.interrupt_logic.lh_target_pos = lh_target_pos interface.interrupt_logic.lh_waypoint_pos = lh_waypoint_pos interface.interrupt_logic.lh_target_quat = lh_target_quat interface.interrupt_logic.b_interrupt_button_one = True waiting_command = False time_command_recved = t command = interface.get_command(copy.deepcopy(sensor_data)) if B_VISUALIZE: p.resetBasePositionAndOrientation(lh_target_frame, lh_target_pos, lh_target_quat) p.resetBasePositionAndOrientation(lh_waypoint_frame, lh_waypoint_pos, lh_target_quat) # Exclude Knee Proximal Joints Command del command['joint_pos']['l_knee_fe_jp'] del command['joint_pos']['r_knee_fe_jp'] del command['joint_vel']['l_knee_fe_jp'] del command['joint_vel']['r_knee_fe_jp'] del command['joint_trq']['l_knee_fe_jp'] del command['joint_trq']['r_knee_fe_jp'] # Apply Command pybullet_util.set_motor_trq(robot, joint_id, command['joint_trq']) pybullet_util.set_motor_pos(robot, joint_id, gripper_command) p.stepSimulation() t += dt count += 1 ## Safety On the run safe_list = is_safe(robot, link_id, sensor_data) if all(safe_list): pass else: safe_list.append(False) del interface break ## Safety at the end if t >= time_command_recved + ManipulationConfig.T_REACHING_DURATION + 0.2: if is_tracking_error_safe(global_goal_pos, angle, robot, link_id): safe_list.append(True) else: safe_list.append(False) del interface break return safe_list
def get_link_iso(robot, link_idx): info = p.getLinkState(robot, link_idx, 1, 1) pos = np.array(info[0]) rot = util.quat_to_rot(np.array(info[1])) return liegroup.RpToTrans(rot, pos)
def get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom): """ Parameters ---------- joint_id (dict): Joint ID Dict link_id (dict): Link ID Dict pos_basejoint_to_basecom (np.ndarray): 3d vector from base joint frame to base com frame rot_basejoint_to_basecom (np.ndarray): SO(3) from base joint frame to base com frame b_fixed_Base (bool); Whether the robot is floating or fixed Returns ------- sensor_data (dict): base_com_pos (np.array): base com pos in world base_com_quat (np.array): base com quat in world base_com_lin_vel (np.array): base com lin vel in world base_com_ang_vel (np.array): base com ang vel in world base_joint_pos (np.array): base pos in world base_joint_quat (np.array): base quat in world base_joint_lin_vel (np.array): base lin vel in world base_joint_ang_vel (np.array): base ang vel in world joint_pos (dict): Joint pos joint_vel (dict): Joint vel b_rf_contact (bool): Right Foot Contact Switch b_lf_contact (bool): Left Foot Contact Switch """ sensor_data = OrderedDict() # Handle Base Frame Quantities base_com_pos, base_com_quat = p.getBasePositionAndOrientation(robot) sensor_data['base_com_pos'] = np.asarray(base_com_pos) sensor_data['base_com_quat'] = np.asarray(base_com_quat) base_com_lin_vel, base_com_ang_vel = p.getBaseVelocity(robot) sensor_data['base_com_lin_vel'] = np.asarray(base_com_lin_vel) sensor_data['base_com_ang_vel'] = np.asarray(base_com_ang_vel) rot_world_com = util.quat_to_rot(np.copy(sensor_data['base_com_quat'])) rot_world_joint = np.dot(rot_world_com, rot_basejoint_to_basecom.transpose()) sensor_data['base_joint_pos'] = sensor_data['base_com_pos'] - np.dot( rot_world_joint, pos_basejoint_to_basecom) sensor_data['base_joint_quat'] = util.rot_to_quat(rot_world_joint) trans_joint_com = liegroup.RpToTrans(rot_basejoint_to_basecom, pos_basejoint_to_basecom) adT_joint_com = liegroup.Adjoint(trans_joint_com) twist_com_in_world = np.zeros(6) twist_com_in_world[0:3] = np.copy(sensor_data['base_com_ang_vel']) twist_com_in_world[3:6] = np.copy(sensor_data['base_com_lin_vel']) augrot_com_world = np.zeros((6, 6)) augrot_com_world[0:3, 0:3] = rot_world_com.transpose() augrot_com_world[3:6, 3:6] = rot_world_com.transpose() twist_com_in_com = np.dot(augrot_com_world, twist_com_in_world) twist_joint_in_joint = np.dot(adT_joint_com, twist_com_in_com) rot_world_joint = np.dot(rot_world_com, rot_basejoint_to_basecom.transpose()) augrot_world_joint = np.zeros((6, 6)) augrot_world_joint[0:3, 0:3] = rot_world_joint augrot_world_joint[3:6, 3:6] = rot_world_joint twist_joint_in_world = np.dot(augrot_world_joint, twist_joint_in_joint) sensor_data['base_joint_lin_vel'] = np.copy(twist_joint_in_world[3:6]) sensor_data['base_joint_ang_vel'] = np.copy(twist_joint_in_world[0:3]) # Joint Quantities sensor_data['joint_pos'] = OrderedDict() sensor_data['joint_vel'] = OrderedDict() for k, v in joint_id.items(): js = p.getJointState(robot, v) sensor_data['joint_pos'][k] = js[0] sensor_data['joint_vel'][k] = js[1] return sensor_data
def sample_swing_config(nominal_lf_iso, nominal_rf_iso, side): swing_time = np.random.uniform(SWING_TIME_LB, SWING_TIME_UB) if side == "left": # Sample rfoot config rfoot_ini_iso = np.copy(nominal_rf_iso) rfoot_mid_iso = np.copy(nominal_rf_iso) rfoot_fin_iso = np.copy(nominal_rf_iso) rfoot_mid_vel = (rfoot_ini_iso[0:3, 3] - rfoot_ini_iso[0:3, 3]) / swing_time rfoot_mid_vel[2] = 0. # Sample lfoot config lfoot_ini_pos = np.copy(nominal_lf_iso)[0:3, 3] + np.random.uniform( LFOOT_POS_LB, LFOOT_POS_UB) lfoot_ini_ea = np.random.uniform(FOOT_EA_LB, FOOT_EA_UB) lfoot_ini_rot = util.euler_to_rot(lfoot_ini_ea) lfoot_ini_iso = liegroup.RpToTrans(lfoot_ini_rot, lfoot_ini_pos) lfoot_fin_pos = np.copy(nominal_lf_iso)[0:3, 3] + np.random.uniform( LFOOT_POS_LB, LFOOT_POS_UB) lfoot_fin_ea = np.random.uniform(FOOT_EA_LB, FOOT_EA_UB) lfoot_fin_rot = util.euler_to_rot(lfoot_fin_ea) lfoot_fin_iso = liegroup.RpToTrans(lfoot_fin_rot, lfoot_fin_pos) lfoot_mid_iso = interpolation.iso_interpolate(lfoot_ini_iso, lfoot_fin_iso, 0.5) lfoot_mid_iso[2, 3] = (lfoot_ini_pos[2] + lfoot_fin_pos[2]) / 2.0 + np.random.uniform( SWING_HEIGHT_LB, SWING_HEIGHT_UB) lfoot_mid_vel = (lfoot_fin_pos - lfoot_ini_pos) / swing_time lfoot_mid_vel[2] = 0. # Sample base config base_ini_pos = (rfoot_ini_iso[0:3, 3] + lfoot_ini_iso[0:3, 3]) / 2.0 base_ini_pos[2] = np.random.uniform(BASE_HEIGHT_LB, BASE_HEIGHT_UB) base_ini_rot = util.euler_to_rot( np.array([0., 0., lfoot_ini_ea[2] / 2.])) base_ini_iso = liegroup.RpToTrans(base_ini_rot, base_ini_pos) base_fin_pos = (rfoot_fin_iso[0:3, 3] + lfoot_fin_iso[0:3, 3]) / 2.0 base_fin_pos[2] = np.random.uniform(BASE_HEIGHT_LB, BASE_HEIGHT_UB) base_fin_rot = util.euler_to_rot( np.array([0., 0., lfoot_fin_ea[2] / 2.])) base_fin_iso = liegroup.RpToTrans(base_fin_rot, base_fin_pos) elif side == "right": # Sample lfoot config lfoot_ini_iso = np.copy(nominal_lf_iso) lfoot_mid_iso = np.copy(nominal_lf_iso) lfoot_fin_iso = np.copy(nominal_lf_iso) lfoot_mid_vel = (lfoot_ini_iso[0:3, 3] - lfoot_ini_iso[0:3, 3]) / swing_time lfoot_mid_vel[2] = 0. # Sample rfoot config rfoot_ini_pos = np.copy(nominal_rf_iso)[0:3, 3] + np.random.uniform( RFOOT_POS_LB, RFOOT_POS_UB) rfoot_ini_ea = np.random.uniform(FOOT_EA_LB, FOOT_EA_UB) rfoot_ini_rot = util.euler_to_rot(rfoot_ini_ea) rfoot_ini_iso = liegroup.RpToTrans(rfoot_ini_rot, rfoot_ini_pos) rfoot_fin_pos = np.copy(nominal_rf_iso)[0:3, 3] + np.random.uniform( RFOOT_POS_LB, RFOOT_POS_UB) rfoot_fin_ea = np.random.uniform(FOOT_EA_LB, FOOT_EA_UB) rfoot_fin_rot = util.euler_to_rot(rfoot_fin_ea) rfoot_fin_iso = liegroup.RpToTrans(rfoot_fin_rot, rfoot_fin_pos) rfoot_mid_iso = interpolation.iso_interpolate(rfoot_ini_iso, rfoot_fin_iso, 0.5) rfoot_mid_iso[2, 3] = (rfoot_ini_pos[2] + rfoot_fin_pos[2]) / 2.0 + np.random.uniform( SWING_HEIGHT_LB, SWING_HEIGHT_UB) rfoot_mid_vel = (rfoot_fin_pos - rfoot_ini_pos) / swing_time rfoot_mid_vel[2] = 0. # Sample base config base_ini_pos = (rfoot_ini_iso[0:3, 3] + lfoot_ini_iso[0:3, 3]) / 2.0 base_ini_pos[2] = np.random.uniform(BASE_HEIGHT_LB, BASE_HEIGHT_UB) base_ini_rot = util.euler_to_rot( np.array([0., 0., rfoot_ini_ea[2] / 2.])) base_ini_iso = liegroup.RpToTrans(base_ini_rot, base_ini_pos) base_fin_pos = (rfoot_fin_iso[0:3, 3] + lfoot_fin_iso[0:3, 3]) / 2.0 base_fin_pos[2] = np.random.uniform(BASE_HEIGHT_LB, BASE_HEIGHT_UB) base_fin_rot = util.euler_to_rot( np.array([0., 0., rfoot_fin_ea[2] / 2.])) base_fin_iso = liegroup.RpToTrans(base_fin_rot, base_fin_pos) else: raise ValueError return swing_time, lfoot_ini_iso, lfoot_mid_iso, lfoot_fin_iso, lfoot_mid_vel, rfoot_ini_iso, rfoot_mid_iso, rfoot_fin_iso, rfoot_mid_vel, base_ini_iso, base_fin_iso
elif pybullet_util.is_key_triggered(keys, '2'): interface.interrupt_logic.b_interrupt_button_two = True elif pybullet_util.is_key_triggered(keys, '6'): interface.interrupt_logic.b_interrupt_button_six = True elif pybullet_util.is_key_triggered(keys, '7'): interface.interrupt_logic.b_interrupt_button_seven = True elif pybullet_util.is_key_triggered(keys, '9'): interface.interrupt_logic.b_interrupt_button_nine = True elif pybullet_util.is_key_triggered(keys, '0'): interface.interrupt_logic.b_interrupt_button_zero = True elif pybullet_util.is_key_triggered(keys, '1'): ## Update target pos and quat here lh_target_pos = np.array([0.6, 0.02, 0.83]) lh_target_rot = np.dot(RIGHTUP_GRIPPER, x_rot(0.)) lh_target_quat = util.rot_to_quat(lh_target_rot) lh_target_iso = liegroup.RpToTrans(lh_target_rot, lh_target_pos) lh_waypoint_pos = generate_keypoint(lh_target_iso)[0:3, 3] print("lh reachability: ", is_lh_reachable(sensor_data, lh_target_pos)) interface.interrupt_logic.lh_target_pos = lh_target_pos interface.interrupt_logic.lh_waypoint_pos = lh_waypoint_pos interface.interrupt_logic.lh_target_quat = lh_target_quat interface.interrupt_logic.b_interrupt_button_one = True elif pybullet_util.is_key_triggered(keys, '3'): ## Update target pos and quat here rh_target_pos = np.array([0.4, 0., 0.83]) rh_target_rot = np.dot(RIGHTUP_GRIPPER, x_rot(0.)) rh_target_quat = util.rot_to_quat(rh_target_rot) rh_target_iso = liegroup.RpToTrans(rh_target_rot, rh_target_pos)