FICO
FICO Xpress Optimization Examples Repository
FICO Optimization Community FICO Xpress Optimization Home
Back to examples browserPrevious 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





grasp_graph.mos

(!*********************************************************************
   Mosel NL examples
   =================
   file grasp_ive.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

   - Graphical representation of results -   

   (c) 2013 Fair Issac Corporation
       author: S. Heipcke, Nov. 2005, rev. Sep. 2017
*********************************************************************!)
    
model "grasping (NL)"
  uses  "mmxnlp", "mmsvg"
   
  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));

!**************** Graphical representation of results ****************

  forall(i in 0..12) pcol(i):= svgcolor(200-10*i,200-10*i,200-10*i)
  svgaddgroup("plot", "Surface", SVG_GRAY)
  svgsetstyle(SVG_OPACITY, 0.35)
  svgsetstyle(SVG_FILL, SVG_CURRENT)

  svgaddgroup("plotC", "Cone", svgcolor(75,75,75))
  svgsetstyle(SVG_OPACITY, 0.5)

  svgaddgroup("plotP", "Lift points", svgcolor(25,25,150))
  svgsetstyle(SVG_STROKEDASH, "1,1")
  svgaddgroup("plotF", "Force", svgcolor(200,25,25))
  svgsetstyle(SVG_STROKEWIDTH, 2)
  svgaddgroup("plotD", "Lifting direction", svgcolor(25,150,25))
  svgsetstyle(SVG_STROKEWIDTH, 2)
  EPS:=0.0001
  FAC:=2.5
  FFAC:=2.5
  
 ! Lifting points and forces
  forall(i in RN) do
    fz:=P(i,1)^2+P(i,2)^2
    fz2:=(P(i,1)+if(abs(force(i,1).sol)>EPS,force(i,1).sol,0))^2+
         (P(i,2)+if(abs(force(i,2).sol)>EPS,force(i,2).sol,0))^2
    svgaddpoint("plotP",P(i,1)+fz/FAC,P(i,2)+fz/FAC)
    svgaddarrow("plotF",P(i,1)+fz/FAC,P(i,2)+fz/FAC,
                 P(i,1)+FFAC*(if(abs(force(i,1).sol)>EPS,force(i,1).sol,0))+fz2/FAC,
                 P(i,2)+FFAC*(if(abs(force(i,2).sol)>EPS,force(i,2).sol,0))+fz2/FAC)
  end-do
                 
 ! Lifting direction
  fz:=(0.3)^2
  svgaddpoint("plotD",1+0.3+fz/FAC,fz/FAC)
  svgaddarrow("plotD",0.3+fz/FAC,fz/FAC,
                 0.3-FFAC*f_ext(1)-f_ext(3)/FAC, -FFAC*f_ext(2)-f_ext(3)/FAC)
 
 ! Circle on which are located the lifting points
  M:=40
  forall(i in 1..M) do
    fz:=(0.3 + cos(2*M_PI*i/M))^2 + (sin(2*M_PI*i/M))^2
    fx:=0.3 + cos(2*M_PI*i/M) + fz/FAC; fy:=sin(2*M_PI*i/M)+fz/FAC
    fz2:=(0.3 + cos(2*M_PI*(i+1)/M))^2 + (sin(2*M_PI*(i+1)/M))^2
    fx2:=0.3 + cos(2*M_PI*(i+1)/M) + fz2/FAC; fy2:=sin(2*M_PI*(i+1)/M)+ fz2/FAC
    svgaddline("plotP",fx,fy,fx2,fy2)
  end-do

 ! Surface of the cone
  forall(y in union(i in -12..12) {i/5})
    forall(x in union(i in -12..12) {i/5}) do
      fz:=(x-0.3)^2+y^2
      fz2:=(x-0.3+0.2)^2+y^2
      fz3:=(x-0.3)^2+(y+0.2)^2
      fz4:=(x-0.3+0.2)^2+(y+0.2)^2
      svgaddpolygon("plot", [x+fz/5,y+fz/5,(x+0.2)+fz2/5,y+fz2/5,
        (x+0.2)+fz4/5,y+0.2+fz4/5,x+fz3/5,y+0.2+fz3/5])
      svgsetstyle(svggetlastobj, SVG_STROKE, pcol(round(abs(x)*5)) )
      svgsetstyle(svggetlastobj, SVG_FILL, pcol(round(abs(x)*5)) )
    end-do

 ! Grid representing the cone
  forall(y in union(i in -12..12) {i/5})
    forall(x in union(i in -12..12) {i/5}) do
      fz:=(x-0.3)^2+y^2
      fz2:=(x-0.3+0.2)^2+y^2
      svgaddline("plotC", x+fz/5,y+fz/5,(x+0.2)+fz2/5,y+fz2/5)
    end-do
 (! forall(x in union(i in -12..12) {i/5})
    forall(y in union(i in -12..12) {i/5}) do
      fz:=(x-0.3)^2+y^2
      fz2:=(x-0.3)^2+(y+0.2)^2
      svgaddline("plotC", x+fz/5,y+fz/5,x+fz2/5,y+0.2+fz2/5)
    end-do
!)

! Scale size of the displayed graph
 svgsetgraphscale(100)
 svgsetgraphlabels("x","y")

 svgsave("grasp.svg")
 svgrefresh
 svgwaitclose("Close browser window to terminate model execution.", 1)
end-model

Back to examples browserPrevious example