FICO
FICO Xpress Optimization Examples Repository
FICO Optimization Community FICO Xpress Optimization Home
Back to examples browserPrevious exampleNext example

Force required to lift an object

Description
Find the smallest amount of force required to lift an object, grasping it at a set of possible grasping points.

Further explanation of this example:


Source Files
By clicking on a file name, a preview is opened at the bottom of this page.
grasp.mos[download]
grasp_graph.mos[download]





grasp.mos

(!*********************************************************************
   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

Back to examples browserPrevious exampleNext example