class ManipulatorInterface(Interface):
def __init__(self):
super(ManipulatorInterface, self).__init__()
self._robot = PinocchioRobotSystem(
cwd + "/robot_model/manipulator/three_link_manipulator.urdf",
cwd + "/robot_model/manipulator", True,
ManipulatorConfig.PRINT_ROBOT_INFO)
def get_command(self, sensor_data):
# Update Robot
self._robot.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"])
# Operational Space Control
jtrq_cmd = self._compute_osc_command()
jpos_cmd = np.zeros_like(jtrq_cmd)
jvel_cmd = np.zeros_like(jtrq_cmd)
# Compute Cmd
command = self._robot.create_cmd_ordered_dict(jpos_cmd, jvel_cmd,
jtrq_cmd)
# Increase time variables
self._count += 1
self._running_time += ManipulatorConfig.DT
return command
def _compute_osc_command(self):
## TODO : Implement Operational Space Control
jtrq = np.zeros(self._robot.n_a)
return jtrq
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
class ManipulatorInterface(Interface):
def __init__(self):
super(ManipulatorInterface, self).__init__()
if ManipulatorConfig.DYN_LIB == "dart":
from pnc.robot_system.dart_robot_system import DartRobotSystem
self._robot = DartRobotSystem(
cwd + "/robot_model/manipulator/three_link_manipulator.urdf",
True, ManipulatorConfig.PRINT_ROBOT_INFO)
elif ManipulatorConfig.DYN_LIB == "pinocchio":
from pnc.robot_system.pinocchio_robot_system import PinocchioRobotSystem
self._robot = PinocchioRobotSystem(
cwd + "/robot_model/manipulator/three_link_manipulator.urdf",
cwd + "/robot_model/manipulator", True,
ManipulatorConfig.PRINT_ROBOT_INFO)
else:
raise ValueError("wrong dynamics library")
self._joint_integrator = JointIntegrator(self._robot.n_a,
ManipulatorConfig.DT)
self._joint_integrator.pos_cutoff_freq = 0.001 # hz
self._joint_integrator.vel_cutoff_freq = 0.002 # hz
self._joint_integrator.max_pos_err = 0.2 # rad
self._joint_integrator.joint_pos_limit = self._robot.joint_pos_limit
self._joint_integrator.joint_vel_limit = self._robot.joint_vel_limit
self._b_first_visit = True
self._data_saver = DataSaver()
def get_command(self, sensor_data):
# Update Robot
self._robot.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"])
if self._b_first_visit:
self._joint_integrator.initialize_states(
self._robot.joint_velocities, self._robot.joint_positions)
self._ini_ee_pos = self._robot.get_link_iso('ee')[0:3, 3]
self._b_first_visit = False
# Operational Space Control
jpos_cmd, jvel_cmd, jtrq_cmd = self._compute_osc_command()
# Compute Cmd
command = self._robot.create_cmd_ordered_dict(jpos_cmd, jvel_cmd,
jtrq_cmd)
# Increase time variables
self._count += 1
self._running_time += ManipulatorConfig.DT
self._data_saver.add('time', self._running_time)
self._data_saver.advance()
return command
def _compute_osc_command(self):
jtrq = np.zeros(self._robot.n_a)
jac = self._robot.get_link_jacobian('ee')[3:6, :]
pos = self._robot.get_link_iso('ee')[0:3, 3]
vel = self._robot.get_link_vel('ee')[3:6]
pos_des = np.zeros(3)
vel_des = np.zeros(3)
acc_des = np.zeros(3)
for i in range(3):
pos_des[i] = interpolation.smooth_changing(
self._ini_ee_pos[i], ManipulatorConfig.DES_EE_POS[i], 3.,
self._running_time)
vel_des[i] = interpolation.smooth_changing_vel(
self._ini_ee_pos[i], ManipulatorConfig.DES_EE_POS[i], 3.,
self._running_time)
acc_des[i] = interpolation.smooth_changing_acc(
self._ini_ee_pos[i], ManipulatorConfig.DES_EE_POS[i], 3.,
self._running_time)
err = pos_des - pos
err_d = vel_des - vel
# xddot_des = acc_des + ManipulatorConfig.KP * err + ManipulatorConfig.KD * err_d
xddot_des = ManipulatorConfig.KP * err + ManipulatorConfig.KD * err_d
qddot_des = np.dot(np.linalg.pinv(jac, rcond=1e-3), xddot_des)
# smoothing qddot
s = interpolation.smooth_changing(0, 1, 0.5, self._running_time)
qddot_des *= s
joint_vel_cmd, joint_pos_cmd = self._joint_integrator.integrate(
qddot_des, self._robot.joint_velocities,
self._robot.joint_positions)
mass_matrix = self._robot.get_mass_matrix()
c = self._robot.get_coriolis()
g = self._robot.get_gravity()
jtrq = np.dot(mass_matrix, qddot_des) + c + g
self._data_saver.add('ee_pos_des', pos_des)
self._data_saver.add('ee_pos_act', pos)
self._data_saver.add('ee_vel_des', vel_des)
self._data_saver.add('ee_vel_act', vel)
self._data_saver.add('ee_acc_des', acc_des)
self._data_saver.add('jpos_des', joint_pos_cmd)
self._data_saver.add('jpos_act', self._robot.joint_positions)
self._data_saver.add('jvel_des', joint_vel_cmd)
self._data_saver.add('jvel_act', self._robot.joint_velocities)
self._data_saver.add('qddot_des', qddot_des)
return joint_pos_cmd, joint_vel_cmd, jtrq
# Arbitrary q and qdot
q = np.array([0.2, 0.1])
qdot = np.array([0.012323, 0.01])
# Update Robot
dart_robot_sys.update_system(None, None, None, None, None, None, None, None, {
'j0': q[0],
'j1': q[1]
}, {
'j0': qdot[0],
'j1': qdot[1]
})
pin_robot_sys.update_system(None, None, None, None, None, None, None, None, {
'j0': q[0],
'j1': q[1]
}, {
'j0': qdot[0],
'j1': qdot[1]
})
# Compare EE SE(3)
print("=" * 80)
print("EE SE3")
print("=" * 80)
print(dart_robot_sys.get_link_iso('ee'))
print(pin_robot_sys.get_link_iso('ee'))
# Compare jacobian
jac_pin = pin_robot_sys.get_link_jacobian('ee')
jac_dart = dart_robot_sys.get_link_jacobian('ee')
an_jac = analytic_jacobian(q)
if VIDEO_RECORD:
pybullet_util.make_video(video_dir, False)
p.disconnect()
exit()
else:
vis_idx += 1
# Visualize config
pybullet_util.set_config(robot, joint_id, link_id, base_pos, base_quat,
joint_pos)
if b_crbi and VIDEO_RECORD:
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)
s_a = np.zeros((robot.n_a - 2, robot.n_a))
act_list = [False] * robot.n_floating + [True] * robot.n_a
act_list[l_jp_idx] = False
act_list[r_jp_idx] = False
j, k = 0, 0
for i in range(robot.n_q_dot):
if act_list[i]:
s_a[j, i] = 1.
j += 1
# print(j_i)
# print(s_a)
# exit()
robot.update_system(np.zeros(3), np.zeros(4), np.zeros(3), np.zeros(3),
np.zeros(3), np.zeros(4), np.zeros(3), np.zeros(3), q,
qdot, True)
mass = robot.get_mass_matrix()
mass_inv = np.linalg.inv(mass)
g = robot.get_gravity()
lambda_i = np.linalg.inv(np.dot(j_i, np.dot(mass_inv, j_i.transpose())))
j_i_bar = np.dot(np.dot(mass_inv, j_i.transpose()), lambda_i)
null_i = np.eye(robot.n_q_dot) - np.dot(j_i_bar, j_i)
grav_comp_cmd = np.dot(
np.dot(np.linalg.pinv(np.dot(s_a, null_i).transpose()),
null_i.transpose()), g)
grav_comp_cmd[3] /= 2.
