def initialize_hand_trajectory(self, start_time, duration,
target_hand_iso):
self._start_moving_time = start_time
self._moving_duration = duration
init_hand_iso = self._robot.get_link_iso(self._target_id)
init_hand_vel = self._robot.get_link_vel(self._target_id)
init_hand_quat = util.rot_to_quat(init_hand_iso[0:3, 0:3])
target_hand_quat = util.rot_to_quat(target_hand_iso[0:3, 0:3])
# self._pos_hermite_curve = interpolation.HermiteCurveVec(
# init_hand_iso[0:3, 3], np.zeros(3), target_hand_iso[0:3, 3],
# np.zeros(3))
self._init_hand_pos = init_hand_iso[0:3, 3]
self._target_hand_pos = target_hand_iso[0:3, 3]
self._quat_hermite_curve = interpolation.HermiteCurveQuat(
init_hand_quat, init_hand_vel[0:3], target_hand_quat, np.zeros(3))
def initialize_floating_base_swaying_trajectory(self, start_time, amp,
freq):
self._start_time = start_time
self._amp = np.copy(amp)
self._freq = np.copy(freq)
self._ini_com_pos = self._robot.get_com_pos()
self._ini_base_quat = util.rot_to_quat(
self._robot.get_link_iso(self._base_ori_task.target_id)[0:3, 0:3])
self._target_base_quat = np.copy(self._ini_base_quat)
def 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):
lfoot_pos_curve_ini_to_mid = interpolation.HermiteCurveVec(
lfoot_ini_iso[0:3, 3], np.zeros(3), lfoot_mid_iso[0:3, 3],
lfoot_mid_vel)
lfoot_pos_curve_mid_to_fin = interpolation.HermiteCurveVec(
lfoot_mid_iso[0:3, 3], lfoot_mid_vel, lfoot_fin_iso[0:3, 3],
np.zeros(3))
lfoot_quat_curve = interpolation.HermiteCurveQuat(
util.rot_to_quat(lfoot_ini_iso[0:3, 0:3]), np.zeros(3),
util.rot_to_quat(lfoot_fin_iso[0:3, 0:3]), np.zeros(3))
rfoot_pos_curve_ini_to_mid = interpolation.HermiteCurveVec(
rfoot_ini_iso[0:3, 3], np.zeros(3), rfoot_mid_iso[0:3, 3],
rfoot_mid_vel)
rfoot_pos_curve_mid_to_fin = interpolation.HermiteCurveVec(
rfoot_mid_iso[0:3, 3], rfoot_mid_vel, rfoot_fin_iso[0:3, 3],
np.zeros(3))
rfoot_quat_curve = interpolation.HermiteCurveQuat(
util.rot_to_quat(rfoot_ini_iso[0:3, 0:3]), np.zeros(3),
util.rot_to_quat(rfoot_fin_iso[0:3, 0:3]), np.zeros(3))
base_pos_curve = interpolation.HermiteCurveVec(base_ini_iso[0:3, 3],
np.zeros(3),
base_fin_iso[0:3, 3],
np.zeros(3))
base_quat_curve = interpolation.HermiteCurveQuat(
util.rot_to_quat(base_ini_iso[0:3, 0:3]), np.zeros(3),
util.rot_to_quat(base_fin_iso[0:3, 0:3]), np.zeros(3))
return 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
def use_current(self):
foot_iso = self._robot.get_link_iso(self._target_id)
foot_vel = self._robot.get_link_vel(self._target_id)
foot_pos_des = foot_iso[0:3, 3]
foot_lin_vel_des = foot_vel[3:6]
self._pos_task.update_desired(foot_pos_des, foot_lin_vel_des,
np.zeros(3))
foot_rot_des = util.rot_to_quat(foot_iso[0:3, 0:3])
foot_ang_vel_des = foot_vel[0:3]
self._ori_task.update_desired(foot_rot_des, foot_ang_vel_des,
np.zeros(3))
def update_desired(self, target_hand_iso):
hand_pos_des = target_hand_iso[0:3, 3]
hand_vel_des = np.zeros(3)
hand_acc_des = np.zeros(3)
hand_ori_des = util.rot_to_quat(target_hand_iso[0:3, 0:3])
hand_ang_vel_des = np.zeros(3)
hand_ang_acc_des = np.zeros(3)
self._pos_task.update_desired(hand_pos_des, hand_vel_des, hand_acc_des)
if self._ori_task is not None:
self._ori_task.update_desired(hand_ori_des, hand_ang_vel_des,
hand_ang_acc_des)
def use_current(self):
current_hand_iso = self._robot.get_link_iso(self._target_id)
current_hand_vel = self._robot.get_link_vel(self._target_id)
hand_pos_des = current_hand_iso[0:3, 3]
hand_vel_des = current_hand_vel[3:6]
hand_acc_des = np.zeros(3)
hand_ori_des = util.rot_to_quat(current_hand_iso[0:3, 0:3])
hand_ang_vel_des = current_hand_vel[0:3]
hand_ang_acc_des = np.zeros(3)
self._pos_task.update_desired(hand_pos_des, hand_vel_des, hand_acc_des)
if self._ori_task is not None:
self._ori_task.update_desired(hand_ori_des, hand_ang_vel_des,
hand_ang_acc_des)
def initialize_floating_base_trajectory(self, start_time, duration,
target_com_pos, target_base_quat):
self._start_time = start_time
self._duration = duration
self._ini_com_pos = self._robot.get_com_pos()
self._target_com_pos = target_com_pos
self._ini_base_quat = util.rot_to_quat(
self._robot.get_link_iso(self._base_ori_task.target_id)[0:3, 0:3])
self._target_base_quat = target_base_quat
self._quat_error = R.from_matrix(
np.dot(
R.from_quat(self._target_base_quat).as_matrix(),
R.from_quat(
self._ini_base_quat).as_matrix().transpose())).as_quat()
self._exp_error = util.quat_to_exp(self._quat_error)
vis_base_lin_pos = vis_base_lin[vis_idx, 0:3]
vis_base_ang_pos = vis_base_ang[vis_idx, 0:3]
vis_ee_motion_lin_pos = dict()
vis_ee_motion_ang_pos = dict()
vis_ee_wrench_lin_pos = dict()
vis_ee_wrench_ang_pos = dict()
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]):
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_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
interface.interrupt_logic.b_interrupt_button_four = True
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)
