23 #include <IpIpoptApplication.hpp>
25 #include <yarp/os/Property.h>
26 #include <yarp/os/LogStream.h>
27 #include <yarp/dev/CartesianControl.h>
28 #include <yarp/sig/Vector.h>
29 #include <yarp/sig/Matrix.h>
30 #include <yarp/math/Math.h>
35 #define RAD2DEG (180.0/M_PI)
36 #define DEG2RAD (M_PI/180.0)
39 using namespace yarp::os;
41 using namespace yarp::sig;
42 using namespace yarp::math;
61 string hand=
info.find(
"arm_type").asString();
65 size_t underscore=hand.find(
'_');
66 if (underscore!=string::npos)
67 hand=hand.substr(0,underscore);
70 for (
unsigned int i=0; i<finger_tip->
getN(); i++)
73 iarm->getLimits(i,&
min,&
max);
104 setFingerJoints(
zeros(16));
136 q.resize(finger_tip->
getN());
137 for (
unsigned int i=0; i<q.length(); i++)
143 bool get_nlp_info(Ipopt::Index&
n, Ipopt::Index& m, Ipopt::Index& nnz_jac_g,
144 Ipopt::Index& nnz_h_lag, IndexStyleEnum& index_style)
150 index_style=TNLP::C_STYLE;
156 Ipopt::Index m, Ipopt::Number* g_l, Ipopt::Number* g_u)
158 for (Ipopt::Index i=0; i<
n; i++)
160 x_l[i]=(*finger_tip->
asChain())(i).getMin();
161 x_u[i]=(*finger_tip->
asChain())(i).getMax();
173 bool init_z, Ipopt::Number* z_L, Ipopt::Number* z_U,
174 Ipopt::Index m,
bool init_lambda, Ipopt::Number* lambda)
176 for (Ipopt::Index i=0; i<
n; i++)
177 x[i]=0.5*((*finger_tip->
asChain())(i).getMin()+
178 (*finger_tip->
asChain())(i).getMax());
185 Vector q(finger_tip->
getDOF());
186 for (
size_t i=0; i<q.length(); i++)
193 bool eval_f(Ipopt::Index
n,
const Ipopt::Number*
x,
bool new_x,
194 Ipopt::Number& obj_value)
202 Ipopt::Number* grad_f)
204 for (Ipopt::Index i=0; i<
n; i++)
205 grad_f[i]=(i==3?2.0*
x[3]:0.0);
210 bool eval_g(Ipopt::Index
n,
const Ipopt::Number*
x,
bool new_x,
211 Ipopt::Index m, Ipopt::Number* g)
216 Vector pb_dir=point-fingerBasePos;
218 Matrix tip=finger_tip->
getH();
219 Vector tb_dir=tip.getCol(3).subVector(0,2)-fingerBasePos;
220 g[0]=
dot(pb_dir,tb_dir)/(
norm(pb_dir)*
norm(tb_dir));
222 double e=-1.0-tip(2,2);
229 Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index* iRow,
230 Ipopt::Index *jCol, Ipopt::Number* values)
235 for (Ipopt::Index i=0; i<
n; i++,
idx++)
240 for (Ipopt::Index i=0; i<
n; i++,
idx++)
250 Vector pb_dir=point-fingerBasePos;
252 Matrix tip=finger_tip->
getH();
253 Vector tb_dir=tip.getCol(3).subVector(0,2)-fingerBasePos;
255 Matrix dpb_dir=-1.0*finger_base->
AnaJacobian().removeRows(3,3);
256 Matrix dtb_dir=finger_tip->
AnaJacobian().removeRows(3,3)+dpb_dir;
258 double npb=
norm(pb_dir);
259 double ntb=
norm(tb_dir);
261 double tmp1=
dot(pb_dir,tb_dir);
265 for (Ipopt::Index i=0; i<
n; i++)
266 values[i]=(
dot(dpb_dir.getCol(i),tb_dir)+
dot(pb_dir,dtb_dir.getCol(i)))/nn-
267 tmp1*(
dot(pb_dir,dpb_dir.getCol(i))*tmp2)/tmp3;
269 double e=-1.0-tip(2,2);
271 for (Ipopt::Index i=0; i<
n; i++)
272 values[
n+i]=-2.0*
e*dtipZ(2,i);
278 bool eval_h(Ipopt::Index
n,
const Ipopt::Number *
x,
bool new_x,
279 Ipopt::Number obj_factor, Ipopt::Index m,
280 const Ipopt::Number *lambda,
bool new_lambda,
281 Ipopt::Index nele_hess, Ipopt::Index *iRow,
282 Ipopt::Index *jCol, Ipopt::Number *values)
289 const Ipopt::Number*
x,
const Ipopt::Number* z_L,
290 const Ipopt::Number* z_U, Ipopt::Index m,
291 const Ipopt::Number* g,
const Ipopt::Number* lambda,
292 Ipopt::Number obj_value,
const Ipopt::IpoptData* ip_data,
293 Ipopt::IpoptCalculatedQuantities* ip_cq)
301 Vector& q, Vector&
x)
305 yError()<<
"Cartesian controller not configured";
309 Bottle *bPoint=requirements.find(
"point").asList();
312 yError()<<
"Target point not provided";
317 for (
int i=0; i<bPoint->size(); i++)
318 point[i]=bPoint->get(i).asFloat64();
320 Ipopt::SmartPtr<Ipopt::IpoptApplication>
app=
new Ipopt::IpoptApplication;
321 app->Options()->SetNumericValue(
"tol",0.01);
322 app->Options()->SetNumericValue(
"constr_viol_tol",0.01);
323 app->Options()->SetIntegerValue(
"acceptable_iter",0);
324 app->Options()->SetStringValue(
"mu_strategy",
"adaptive");
325 app->Options()->SetIntegerValue(
"max_iter",100);
326 app->Options()->SetStringValue(
"nlp_scaling_method",
"gradient-based");
327 app->Options()->SetStringValue(
"hessian_approximation",
"limited-memory");
328 app->Options()->SetStringValue(
"derivative_test",
"none");
329 app->Options()->SetIntegerValue(
"print_level",0);
335 nlp->setPoint(point);
336 if (Bottle *bFingerJoints=requirements.find(
"finger-joints").asList())
338 Vector finger_joints(bFingerJoints->size());
339 for (
size_t i=0; i<finger_joints.length(); i++)
340 finger_joints[i]=bFingerJoints->get(i).asFloat64();
341 nlp->setFingerJoints(finger_joints);
344 Ipopt::ApplicationReturnStatus status=
app->OptimizeTNLP(GetRawPtr(nlp));
353 if ((q.length()>=10) && (
x.length()>=7))
356 iarm->storeContext(&context);
360 iarm->setDOF(dof,dof);
361 for (
unsigned int i=0; i<q.length(); i++)
362 iarm->setLimits(i,q[i],q[i]);
365 iarm->getTrajTime(&traj);
366 iarm->setInTargetTol(0.02);
367 iarm->goToPoseSync(
x.subVector(0,2),
x.subVector(3,6));
368 iarm->waitMotionDone(0.2,5.0*traj);
371 iarm->restoreContext(context);
372 iarm->deleteContext(context);
bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number *x, bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U, Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
PointingFarNLP(ICartesianControl *iarm)
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
virtual ~PointingFarNLP()
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number obj_factor, Ipopt::Index m, const Ipopt::Number *lambda, bool new_lambda, Ipopt::Index nele_hess, Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Number *g)
void setFingerJoints(const Vector &finger_joints)
void setAng(const Ipopt::Number *x)
void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n, const Ipopt::Number *x, const Ipopt::Number *z_L, const Ipopt::Number *z_U, Ipopt::Index m, const Ipopt::Number *g, const Ipopt::Number *lambda, Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data, Ipopt::IpoptCalculatedQuantities *ip_cq)
void setPoint(const Vector &point)
bool get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt::Number *x_u, Ipopt::Index m, Ipopt::Number *g_l, Ipopt::Number *g_u)
bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
void getResult(Vector &q, Vector &x) const
bool eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
A class for defining the iCub Arm.
A class for defining the iCub Finger.
virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders, yarp::sig::Vector &chainJoints)
Retrieves the vector of actual finger's joint values (to be used in conjuction with the iKinLimb meth...
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
bool setHN(const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
yarp::sig::Vector EndEffPosition()
Returns the 3D coordinates of end-effector position.
yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col)
Returns the analitical Jacobian of the ith link.
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
yarp::sig::Vector getAng()
Returns the current free joint angles values.
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
yarp::sig::Matrix getH0() const
Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
unsigned int getN() const
Returns the number of Links belonging to the Chain.
unsigned int getDOF() const
Returns the current number of Chain's DOF.
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
static uint32_t idx[BOARD_NUM]
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
Returns the dot product between two vectors given in the form: matrix(:,col).
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
static bool compute(yarp::dev::ICartesianControl *iarm, const yarp::os::Property &requirements, yarp::sig::Vector &q, yarp::sig::Vector &x)
static bool point(yarp::dev::ICartesianControl *iarm, const yarp::sig::Vector &q, const yarp::sig::Vector &x)