unified_manipulation/objFun.m at main · grablab/unified_manipulation · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
function val = objFun(config)

     % % ALPHA weights
    w_i = [40 30 ...    % obj position, angle
            2 1 0.1 ...   % manip, joint lim, collide
            0.1 ...     % WIHM
            0.1 ...     % gravity torque
            2 3 4 ... % motion smoothness
            0.1];           % object jerk


    % BETA weights
%     w_i = [7 5 ...    % obj position, angle
%             2 1 0.1 ...   % manip, joint lim, collide
%             0.1 ...     % WIHM
%             0.1 ...     % gravity torque
%             6 5 4 ... % motion smoothness
%             2];           % object jerk

    para.n = 1;         % parameters for grooveLoss
    para.s = 0;
    para.c = 0.1;
    para.r = 7;

    global robstruct
    robstruct.w_i = w_i;
    robstruct.para = para;

    w_i = w_i./norm(w_i);

    val =  w_i(1)*grooveLoss(objFun_ObjectPosition(config), para) + ...
           w_i(2)*grooveLoss(objFun_ObjectRotation(config), para) + ...
           w_i(3)*grooveLoss(objFun_Manipulability(config), para) + ...
           w_i(4)*grooveLoss(objFun_JointLimits(config), para) + ... %
           w_i(5)*grooveLoss(objFun_Collisions(config), para) + ...
           w_i(6)*grooveLoss(objFun_WIHM(config), para) + ...
           w_i(7)*grooveLoss(objFun_ArmGravityTorque(config), para) + ...
           w_i(8)*grooveLoss(objFun_ArmVelocity(config), para) + ...
           w_i(9)*grooveLoss(objFun_ArmAcceleration(config), para) + ...
           w_i(10)*grooveLoss(objFun_ArmJerk(config), para) +...
           w_i(11)*grooveLoss(objFun_ObjectJerk(config), para) ;

end


%{

w_i index dictionary:

1 - Object position
2 - Object orientation

3 - Arm Singularity
4 - Arm Joint Limits
5 - Arm Collisions

6 - WIHM Motion required (Accuracy / Likelihood of dropping object)

7 - Arm Gravity Torques

8 - Arm Joint Velocity
9 - Arm Joint Acceleration
10- Arm Joint Jerk

11- Object Jerk

%}