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