FICO Xpress Optimization Examples Repository
 FICO Optimization Community FICO Xpress Optimization Home

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_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)
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)
svgsetstyle(SVG_OPACITY, 0.35)
svgsetstyle(SVG_FILL, SVG_CURRENT)

svgsetstyle(SVG_OPACITY, 0.5)

svgsetstyle(SVG_STROKEDASH, "1,1")
svgsetstyle(SVG_STROKEWIDTH, 2)
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
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
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
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
(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
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
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