replayTrajectory.m

%% PLot the resulting trajectory, and export it to a format ACES can use
hipOffset=45*pi/180;
raw1=importdata('step1traj.txt',' ',3);
raw2=importdata('step2traj.txt',' ',3);
HY=initState(2827)
%Column order is, as best I can tell based on the initial pose:
% t HY RSP RSR RSY REP RWY RWR RWP LSP LSR LSY LEP LWY LWR LWP <Robot overall rotation quaternion, 4> <Robot overall translation vector, 3>

% prune necessary trajectory data
%ind=[1:10:end,end];
skips=5;
graspTraj=raw1.data([1:skips:end,end],[3:7,10:14]);
boxLiftTraj=raw2.data([1:skips:end,end],[3:7,10:14]);
initialPose=graspTraj(1,:);
dt=mean(diff(raw1.data([1:skips:end,end],1)));
slowFactor=2;

stateVector=initBasicHubo(2827+19);

disp('initializing webots model pose');
cmdHuboBasic(stateVector,zeros(14,1))
safeCmd(HY,0);
pause
disp('clear the pole first')
cmdHuboBasic(stateVector,[0 .3 0 0.6 0 0 .3 0 0.6 0])

pause
safeCmd(HY,hipOffset);
pause
disp('Setting grasp initial pose, Press any key to continue...');
cmdHuboBasic(stateVector,initialPose)
pause(2);
safeCmd(HY,hipOffset);

pause;
%execute the trajectories generated by the planner, run the webots model side-by-side
%orProblemSendCommand(['traj cmovetraj.txt'],probs.cbirrt);
%orEnvWait(1);

disp('Beginning grasp trajectory (ends with hands closed around box)')

for k=1:size(graspTraj,1)
    cmdHuboBasic(stateVector,graspTraj(k,:))
    pause(dt*slowFactor);
    disp(k)
end

pause
disp('Beginning lift trajectory')
for k=1:size(boxLiftTraj,1)
    cmdHuboBasic(stateVector,boxLiftTraj(k,:))
    pause(dt*slowFactor);
    disp(k)
end

pause(1)

disp('Rotating Hip')
safeCmd(HY,-hipOffset)
pause(2);
RSY=initState(2827+19+3);
LSY=initState(2827+19+10);
safeCmd(RSY,0);
pause(.02);
safeCmd(LSY,0);

%Special Zeroing
LSR=initState(2827+19+9);
LEP=initState(2827+19+11);
LSP=initState(2827+19+8);
pause
safeCmd(HY,0);
pause
safeCmd(LSR,1.2);
pause
safeCmd(LEP,0);
pause
safeCmd(LSP,0);
pause
trunkZero