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 get_robot_config(robot, initial_pos=None, initial_quat=None, b_print_info=False): nq, nv, na, joint_id, link_id = 0, 0, 0, OrderedDict(), OrderedDict() link_id[(p.getBodyInfo(robot)[0]).decode("utf-8")] = -1 for i in range(p.getNumJoints(robot)): info = p.getJointInfo(robot, i) if info[2] != p.JOINT_FIXED: joint_id[info[1].decode("utf-8")] = info[0] link_id[info[12].decode("utf-8")] = info[0] nq = max(nq, info[3]) nv = max(nv, info[4]) nq += 1 nv += 1 na = len(joint_id) base_pos, base_quat = p.getBasePositionAndOrientation(robot) rot_world_com = util.quat_to_rot(base_quat) initial_pos = [0., 0., 0.] if initial_pos is None else initial_pos initial_quat = [0., 0., 0., 1.] if initial_quat is None else initial_quat rot_world_basejoint = util.quat_to_rot(np.array(initial_quat)) pos_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), base_pos - np.array(initial_pos)) rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), rot_world_com) if b_print_info: print("=" * 80) print("SimulationRobot") print("nq: ", nq, ", nv: ", nv, ", na: ", na) print("Vector from base joint frame to base com frame") print(pos_basejoint_to_basecom) print("Rotation from base joint frame to base com frame") print(rot_basejoint_to_basecom) print("+" * 80) print("Joint Infos") util.pretty_print(joint_id) print("+" * 80) print("Link Infos") util.pretty_print(link_id) return nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom
def first_visit(self): self._start_time = self._sp.curr_time if self._state_id == LocomanipulationState.RH_HANDREACH: # print("[LocomanipulationState] Right Hand Reaching") target_hand_iso = np.eye(4) target_hand_iso[0:3, 0:3] = util.quat_to_rot(self._rh_target_quat) target_hand_iso[0:3, 3] = self._rh_target_pos # self._trajectory_managers['rhand'].initialize_hand_trajectory( # self._start_time, self._moving_duration, target_hand_iso) self._trajectory_managers[ 'rhand'].initialize_keypoint_hand_trajectory( self._start_time, self._moving_duration, self._rh_waypoint_pos, target_hand_iso) self._hierarchy_managers["rhand_pos"].initialize_ramp_to_max( self._sp.curr_time, self._trans_duration) self._hierarchy_managers["rhand_ori"].initialize_ramp_to_max( self._sp.curr_time, self._trans_duration) elif self._state_id == LocomanipulationState.LH_HANDREACH: # print("[LocomanipulationState] Left Hand Reaching") target_hand_iso = np.eye(4) target_hand_iso[0:3, 0:3] = util.quat_to_rot(self._lh_target_quat) target_hand_iso[0:3, 3] = self._lh_target_pos # self._trajectory_managers['lhand'].initialize_hand_trajectory( # self._start_time, self._moving_duration, target_hand_iso) self._trajectory_managers[ 'lhand'].initialize_keypoint_hand_trajectory( self._start_time, self._moving_duration, self._lh_waypoint_pos, target_hand_iso) self._hierarchy_managers["lhand_pos"].initialize_ramp_to_max( self._sp.curr_time, self._trans_duration) self._hierarchy_managers["lhand_ori"].initialize_ramp_to_max( self._sp.curr_time, self._trans_duration) else: raise ValueError("Wrong LocomanipulationState: HandSide")
def update_system(self, base_com_pos, base_com_quat, base_com_lin_vel, base_com_ang_vel, base_joint_pos, base_joint_quat, base_joint_lin_vel, base_joint_ang_vel, joint_pos, joint_vel, b_cent=False): assert len(joint_pos.keys()) == self._n_a self._q = np.zeros(self._n_q) self._q_dot = np.zeros(self._n_q_dot) self._joint_positions = np.zeros(self._n_a) self._joint_velocities = np.zeros(self._n_a) if not self._b_fixed_base: # Floating Based Robot self._q[0:3] = np.copy(base_joint_pos) self._q[3:7] = np.copy(base_joint_quat) rot_w_basejoint = util.quat_to_rot(base_joint_quat) twist_basejoint_in_world = np.zeros(6) twist_basejoint_in_world[0:3] = base_joint_ang_vel twist_basejoint_in_world[3:6] = base_joint_lin_vel augrot_joint_world = np.zeros((6, 6)) augrot_joint_world[0:3, 0:3] = rot_w_basejoint.transpose() augrot_joint_world[3:6, 3:6] = rot_w_basejoint.transpose() twist_basejoint_in_joint = np.dot(augrot_joint_world, twist_basejoint_in_world) self._q_dot[0:3] = twist_basejoint_in_joint[3:6] self._q_dot[3:6] = twist_basejoint_in_joint[0:3] else: # Fixed Based Robot pass self._q[self.get_q_idx(list(joint_pos.keys()))] = np.copy( list(joint_pos.values())) self._q_dot[self.get_q_dot_idx(list(joint_vel.keys()))] = np.copy( list(joint_vel.values())) self._joint_positions[self.get_joint_idx(list( joint_pos.keys()))] = np.copy(list(joint_pos.values())) self._joint_velocities[self.get_joint_idx(list( joint_vel.keys()))] = np.copy(list(joint_vel.values())) pin.forwardKinematics(self._model, self._data, self._q, self._q_dot) if b_cent: self._update_centroidal_quantities()
sensor_data["base_joint_lin_vel"], sensor_data["base_joint_ang_vel"], sensor_data["joint_pos"], sensor_data["joint_vel"], True) print("=" * 80) print("Step : ", step) print("-" * 80) target_link = 'r_sole' ls = p.getLinkState(robot, link_id[target_link], 1, 1) pb_lin_vel = np.array(ls[6]) pb_ang_vel = np.array(ls[7]) pb_spat_vel = np.concatenate([pb_ang_vel, pb_lin_vel], axis=0) print("Link Pos from PyBullet") print(np.array(ls[0])) print("Link Rot from PyBullet") print(util.quat_to_rot(np.array(ls[1]))) print("Link Vel from PyBullet") print(pb_spat_vel) pnc_iso = robot_sys.get_link_iso(target_link) pnc_spat_vel = robot_sys.get_link_vel(target_link) pnc_qdot = robot_sys.get_q_dot() pnc_jac = robot_sys.get_link_jacobian(target_link) pnc_jacdot_times_qdot = robot_sys.get_link_jacobian_dot_times_qdot(target_link) pnc_com = robot_sys.get_com_pos() pnc_com_vel = robot_sys.get_com_lin_vel() pnc_com_jac = robot_sys.get_com_lin_jacobian() pnc_com_jac_dot = robot_sys.get_com_lin_jacobian_dot() pnc_grav = robot_sys.get_gravity() pnc_cor = robot_sys.get_coriolis() pnc_mass = robot_sys.get_mass_matrix()
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 _do_generate_data(n_data, nominal_lf_iso, nominal_rf_iso, nominal_sensor_data, side, rseed=None, cpu_idx=0): if rseed is not None: np.random.seed(rseed) from pnc.robot_system.pinocchio_robot_system import PinocchioRobotSystem robot_sys = PinocchioRobotSystem(cwd + "/robot_model/atlas/atlas.urdf", cwd + "/robot_model/atlas", False, False) data_x, data_y = [], [] text = "#" + "{}".format(cpu_idx).zfill(3) with tqdm(total=n_data, desc=text + ': Generating data', position=cpu_idx + 1) as pbar: for i in range(n_data): 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 = sample_swing_config( nominal_lf_iso, nominal_rf_iso, side) lfoot_pos_curve_ini_to_mid, lfoot_pos_curve_mid_to_fin, lfoot_quat_curve, rfoot_pos_curve_ini_to_mid, rfoot_pos_curve_mid_to_fin, rfoot_quat_curve, base_pos_curve, base_quat_curve = create_curves( 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) for s in np.linspace(0, 1, N_DATA_PER_MOTION): base_pos = base_pos_curve.evaluate(s) base_quat = base_quat_curve.evaluate(s) if s <= 0.5: sprime = 2.0 * s lf_pos = lfoot_pos_curve_ini_to_mid.evaluate(sprime) rf_pos = rfoot_pos_curve_ini_to_mid.evaluate(sprime) else: sprime = 2.0 * (s - 0.5) lf_pos = lfoot_pos_curve_mid_to_fin.evaluate(sprime) rf_pos = rfoot_pos_curve_mid_to_fin.evaluate(sprime) lf_quat = lfoot_quat_curve.evaluate(s) rf_quat = rfoot_quat_curve.evaluate(s) joint_pos, lf_done, rf_done = 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) if lf_done and rf_done: rot_world_com = util.quat_to_rot(np.copy(base_quat)) rot_world_joint = np.dot( rot_world_com, rot_basejoint_to_basecom.transpose()) base_joint_pos = base_pos - np.dot( rot_world_joint, pos_basejoint_to_basecom) base_joint_quat = util.rot_to_quat(rot_world_joint) robot_sys.update_system(base_pos, base_quat, np.zeros(3), np.zeros(3), base_joint_pos, base_joint_quat, np.zeros(3), np.zeros(3), joint_pos, nominal_sensor_data['joint_vel'], True) world_I = robot_sys.Ig[0:3, 0:3] local_I = np.dot( np.dot(rot_world_com.transpose(), world_I), rot_world_com) # append to data data_x.append( np.concatenate([lf_pos - base_pos, rf_pos - base_pos], axis=0)) data_y.append( np.array([ local_I[0, 0], local_I[1, 1], local_I[2, 2], local_I[0, 1], local_I[0, 2], local_I[1, 2] ])) pbar.update(1) return data_x, data_y
joint_pos) # Get SensorData sensor_data = pybullet_util.get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom) if b_regressor_trained: robot_sys.update_system( sensor_data["base_com_pos"], sensor_data["base_com_quat"], sensor_data["base_com_lin_vel"], sensor_data["base_com_ang_vel"], sensor_data["base_joint_pos"], sensor_data["base_joint_quat"], sensor_data["base_joint_lin_vel"], sensor_data["base_joint_ang_vel"], sensor_data["joint_pos"], sensor_data["joint_vel"], True) rot_world_base = util.quat_to_rot(sensor_data['base_com_quat']) world_I = robot_sys.Ig[0:3, 0:3] local_I = np.dot(np.dot(rot_world_base.transpose(), world_I), rot_world_base) rf_iso = pybullet_util.get_link_iso(robot, link_id["r_sole"]) lf_iso = pybullet_util.get_link_iso(robot, link_id["l_sole"]) denormalized_output, output = evaluate_crbi_model_using_tf( crbi_model, sensor_data["base_com_pos"], lf_iso[0:3, 3], rf_iso[0:3, 3], input_mean, input_std, output_mean, output_std) local_I_est = inertia_from_one_hot_vec(denormalized_output) if b_ik: data_saver.add('gt_inertia', inertia_to_one_hot_vec(local_I)) data_saver.add( 'gt_inertia_normalized',