testReachingSphere.m

%load the robot into the environment
orEnvLoadScene('openHubo/jaemiHubo.robot.xml',1);
robotid = orEnvGetBody('jaemiHubo');

%set printing and display options
orEnvSetOptions('debug 3')
orEnvSetOptions('collision ode')

manips = orRobotGetManipulators(robotid);

activedofs = [manips{2}.armjoints];
orRobotSetActiveDOFs(robotid,activedofs);
orRobotSetActiveDOFs(robotid,activedofs);

objectid = orEnvCreateKinBody('rightPalmShell','openHubo/testBall.kinbody.xml');

%create the problem instances we need
probs.cbirrt = orEnvCreateProblem('CBiRRT','jaemiHubo');

%get the descriptions of the robot's manipulators
manips = orRobotGetManipulators(robotid);

orRobotSetDOFValues(robotid,[0 pi/3 0 0 0 0 0],manips{1}.armjoints);
%set initial configuration
initDOFValues = [.1 -.5 0 -1 0 0 0];
T_away=[eye(3),[2;2;2];0 0 0 1];

for k=1:50
    %Prepare for a new iteration:
    orBodySetTransform(objectid, [GetRot(T_away) GetTrans(T_away)]');
    tscale=[.2 .2 .1]';

    %Center of random distribution as position wrt torso
    tcenter=[.26 -.05 -.15]';
    t_can=rand(3,1).*tscale-tscale/2+tcenter;

    palm_translation =-[0 0.06 0];
    Tw_e1 = MakeTransform(eye(3),palm_translation');

    T0_object = MakeTransform(eye(3),t_can);
    T0_w=T0_object;
    orRobotSetDOFValues(robotid,initDOFValues,activedofs);
    orBodySetTransform(objectid, [GetRot(T0_object) GetTrans(T0_object)]');

    orEnvWait();
    pause(1)

    orBodySetTransform(objectid, [GetRot(T0_w) GetTrans(T0_w)]');
    pause(.5)

    %Allow a large range of rotations to reach the goal, just to see if the planner can do this
    Bw = [0 0 0 0 0 0 -1 1 -1 1 -1 1];
    orRobotSetDOFValues(robotid,initDOFValues,activedofs);
    %orBodySetTransform(objectid, [GetRot(T_away) GetTrans(T_away)]');

    TSRstring1 = SerializeTSR(1,'NULL',T0_w,Tw_e1,Bw);
    TSRChainString1 = SerializeTSRChain(0,1,0,1,TSRstring1,'NULL',[]);

    %now define the second TSR chain
    %it is the same as the first TSR Chain except Tw_e is different (the hand is rotated by 180 degrees about its z axis)
    Tw_e2 = MakeTransform(rodrigues([0 pi 0])*rodrigues([pi/2 0 0]),[0 0 0]');

    TSRstring2 = SerializeTSR(1,'NULL',T0_w,Tw_e2,Bw);
    TSRChainString2 = SerializeTSRChain(0,1,0,1,TSRstring2,'NULL',[]);

    %call the cbirrt planner, it will generate a file with the trajectory called 'cmovetraj.txt'
    soln=orProblemSendCommand(['RunCBiRRT psample 0.15 timelimit 30' TSRChainString1 ' '  TSRChainString2],probs.cbirrt)
    if strcmp(soln,'1')
        %execute the trajectories generated by the planner
        orProblemSendCommand(['traj cmovetraj.txt'],probs.cbirrt);
        orEnvWait(2);
        pause(2)
    end
end