(!*********************************************************************
   Mosel NL examples
   =================
   file grasp.mos
   ``````````````
   Find the smallest amount of normal force required
   to "grasp" an object given a set of possible grasping points.

   SOCP formulation.

   Based on grasp.mod, gasp_exp.mod, grasp_nonconvex.mod 
   Source: http://www.orfe.princeton.edu/~rvdb/ampl/nlmodels/grasp/ 
   Reference: 
   "Applications of Second-Order Cone Programming",
   M.S. Lobo, L. Vandenberghe, S. Boyd, and H. Lebret, 1998

   (c) 2013 Fair Issac Corporation
       author: S. Heipcke, Nov. 2005, rev. Sep. 2013
*********************************************************************!)
    
model "grasping (NL)"
  uses  "mmxnlp"
   
  parameters
    N = 6                          ! Number of lifting points
    MU = 0.3                       ! Friction coefficient
  end-parameters

  declarations
    RN = 1..N                      ! Set of lifting points
    DIM = 1..3                     ! Set of dimensions
    P: array(RN,DIM) of real       ! Contact point
    GRAD_NORM: array(RN) of real   ! Auxiliary term
    V: array(RN,DIM) of real       ! Unit normal vector at contact point
    f_ext: array(DIM) of real      ! Externally applied force
    torq_ext: array(DIM) of real   ! Externally applied torque

    force: array(RN,DIM) of mpvar  ! Contact force at point
    nforce: array(RN) of mpvar     ! Normal force at point
    munforce: array(RN) of mpvar   ! Aux. var for SOCP reformulation
    tforce: array(RN,DIM) of mpvar ! Tangential force at point
    torq: array(RN,DIM) of mpvar   ! Torque at point
    pressure: mpvar                ! Objective variable, maximum of nforce
    Friction: array(RN) of nlctr   ! Friction relation
  end-declarations

! Defining bounds and start values
  pressure is_free
    
  forall (d in DIM, i in RN) do
    force(i,d) <= 10
    tforce(i,d) <= 2
    torq(i,d) <= 10
    force(i,d) >= -10
    tforce(i,d) >= -10
    torq(i,d) >= -10
    setinitval(force(i,d), 1.0)
  end-do
  forall (i in RN) nforce(i) >= 0

  f_ext :: [0.0, 0.0, -1.0]
  forall(d in DIM) torq_ext(d) := 0.0

! Calculate parameters
  forall(i in RN) do
  ! P(i) is a contact point on a parabolic "nose cone" to be lifted
    P(i,1) := 0.3 + cos(2*M_PI*i/N)
    P(i,2) :=       sin(2*M_PI*i/N)
    P(i,3) := P(i,1)^2 + P(i,2)^2
    GRAD_NORM(i) := sqrt( (2*P(i,1))^2 + (2*P(i,2))^2 + 1 )
  ! V(i) is the unit normal vector at contact point P(i) 
    V(i,1) := -2*P(i,1)/GRAD_NORM(i)
    V(i,2) := -2*P(i,2)/GRAD_NORM(i)
    V(i,3) := 1/GRAD_NORM(i)
  end-do

! Constraints:
  forall(i in RN) do
   ! Normal force at point P(i)
    nfDef(i):= nforce(i) = sum(d in DIM) V(i,d)*force(i,d)
    
   ! Tangential force at point P(i) 
    forall(d in DIM)
      tfDef(i,d):= tforce(i,d) = force(i,d) - V(i,d)*nforce(i)

   ! Torq about (0,0,0) at point P(i)
    torq1Def(i):= torq(i,1) = P(i,2)*force(i,3) - force(i,2)*P(i,3)
    torq2Def(i):= torq(i,2) = P(i,3)*force(i,1) - force(i,3)*P(i,1)
    torq3Def(i):= torq(i,3) = P(i,1)*force(i,2) - force(i,1)*P(i,2)

  ! Objective function definition
    t_bnds(i) := nforce(i) <= pressure

  ! Definition of friction
    munforce(i)=MU*nforce(i)
    Friction(i):= sum(d in DIM) tforce(i,d)^2  <= munforce(i)^2 
  end-do
 
! Force balances
  forall(d in DIM) f_Balance(d) := sum(i in RN) force(i,d) = -f_ext(d)
  forall(d in DIM) t_Balance(d) := sum(i in RN) torq(i,d) = -torq_ext(d)

! Solving the problem
  setparam("xnlp_verbose", true)
  minimize(pressure)

! Solution display 
  setparam("REALFMT", "%7.4f")
  forall(i in RN) do
    write("force(",i,")           = ")
    forall(d in DIM) write(getsol(force(i,d)), " ") 
    write("\ntorque(",i,")          = ")
    forall(d in DIM) write(getsol(torq(i,d)), " ")
    writeln("\nnormal force(",i,")    = ", getsol(nforce(i))) 
    write("tangential force(",i,")= ")
    forall(d in DIM) write(getsol(tforce(i,d)), " ") 
    writeln
  end-do
    
  writeln("\n Pressure = ", getsol(pressure));

end-model