int kasumi_ecb_decrypt(const unsigned char *ct, unsigned char *pt, symmetric_key *skey)
{
ulong32 left, right, temp;
int n;
LTC_ARGCHK(pt != NULL);
LTC_ARGCHK(ct != NULL);
LTC_ARGCHK(skey != NULL);
LOAD32H(left, ct);
LOAD32H(right, ct+4);
for (n = 7; n >= 0; ) {
temp = FO(right, n, skey);
temp = FL(temp, n--, skey);
left ^= temp;
temp = FL(left, n, skey);
temp = FO(temp, n--, skey);
right ^= temp;
}
STORE32H(left, pt);
STORE32H(right, pt+4);
return CRYPT_OK;
}
/*! One possible formulation of the core GFO problem. Checks if some
combination of joint forces exists so that the resultant object
wrench is 0. See inner comments for exact mathematical formulation.
Not for standalone use; is called by the GFO functions in the
Grasp class.
*/
int
graspForceExistence(Matrix &JTD, Matrix &D, Matrix &F, Matrix &G,
Matrix &beta, Matrix &tau, double *objVal)
{
// exact problem formulation
// unknowns: [beta tau] (contact forces and joint forces)
// minimize [beta tau]T*[G 0]T*[G 0]*[beta tau] (magnitude of resultant object wrench)
// subject to:
// [JTD -I] * [beta tau] = 0 (contact forces balance joint forces)
// [0 sum] * [beta tau] = 1 (we are applying some joint forces)
// [F 0] [beta tau] <= 0 (all forces inside friction cones)
// [beta tau] >= 0 (all forces must be positive)
// overall equality constraint:
// | JTD -I | | beta | |0|
// | 0 sum| | tau | = |1|
int numJoints = tau.rows();
Matrix beta_tau(beta.rows() + tau.rows(), 1);
//right-hand side of equality constraint
Matrix right_hand( JTD.rows() + 1, 1);
right_hand.setAllElements(0.0);
//actually, we use 1.0e9 here as units are in N * 1.0e-6 * mm
//so we want a total joint torque of 1000 N mm
right_hand.elem( right_hand.rows()-1, 0) = 1.0e10;
//left-hand side of equality constraint
Matrix LeftHand( JTD.rows() + 1, D.cols() + numJoints);
LeftHand.setAllElements(0.0);
LeftHand.copySubMatrix(0, 0, JTD);
LeftHand.copySubMatrix(0, D.cols(), Matrix::NEGEYE(numJoints, numJoints) );
for (int i=0; i<numJoints; i++) {
LeftHand.elem( JTD.rows(), D.cols() + i) = 1.0;
}
//matrix F padded with zeroes for tau
//left hand side of the inequality matrix
Matrix FO(F.rows(), F.cols() + numJoints);
FO.setAllElements(0.0);
FO.copySubMatrix(0, 0, F);
//right-hand side of inequality matrix
Matrix inEqZeroes(FO.rows(), 1);
inEqZeroes.setAllElements(0.0);
//objective matrix: G padded with zeroes
Matrix GO(Matrix::ZEROES<Matrix>(G.rows(), G.cols() + numJoints));
GO.copySubMatrix(0, 0, G);
//bounds: all variables greater than 0
// CHANGE: only beta >= 0, tau is not
Matrix lowerBounds(Matrix::MIN_VECTOR(beta_tau.rows()));
lowerBounds.copySubMatrix( 0, 0, Matrix::ZEROES<Matrix>(beta.rows(), 1) );
Matrix upperBounds(Matrix::MAX_VECTOR(beta_tau.rows()));
/*
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"left hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
FO.print(fp);
fprintf(fp,"Objective:\n");
GO.print(fp);
fclose(fp);
*/
// assembled system:
// minimize beta_tauT*QOT*QO*beta_tau subject to:
// LeftHand * beta_tau = right_hand
// FO * beta_tau <= inEqZeroes
// beta_tau >= 0
// CHANGE: only beta >= 0, tau is not
int result = factorizedQPSolver(GO, LeftHand, right_hand, FO, inEqZeroes,
lowerBounds, upperBounds,
beta_tau, objVal);
beta.copySubBlock(0, 0, beta.rows(), 1, beta_tau, 0, 0);
tau.copySubBlock(0, 0, tau.rows(), 1, beta_tau, beta.rows(), 0);
return result;
}
/*! One possible formulation of the core GFO problem. Finds the joint
forces that result in 0 object wrench such that contact forces are as
far as possible from the edges of the friction cones. Assumes that at
least one set of contact forces that satisfy this criterion exist; see
contactForceExistence for that problem. See inner comments for exact
mathematical formulation. Not for standalone use; is called by the GFO
functions in the Grasp class.
*/
int
graspForceOptimization(Matrix &JTD, Matrix &D, Matrix &F, Matrix &G,
Matrix &beta, Matrix &tau, double *objVal)
{
// exact problem formulation
// unknowns: [beta tau] (contact forces and joint forces)
// minimize [sum] [F 0] [beta tau] (sort of as far inside the friction cone as possible, not ideal)
// subject to:
// [JTD -I] * [beta tau] = 0 (contact forces balance joint forces)
// [G 0]* [beta tau] = 0 (0 resultant object wrench)
// [0 sum] * [beta tau] = 1 (we are applying some joint forces)
// [F 0] [beta tau] <= 0 (all forces inside friction cones)
// [beta tau] >= 0 (all forces must be positive)
// overall equality constraint:
// | JTD -I | | beta | |0|
// | G 0 | | tau | = |0|
// | 0 sum| |1|
Matrix beta_tau(beta.rows() + tau.rows(), 1);
int numJoints = tau.rows();
//right-hand side of equality constraint
Matrix right_hand( JTD.rows() + G.rows() + 1, 1);
right_hand.setAllElements(0.0);
//actually, we use 1.0e8 here as units are in N * 1.0e-6 * mm
//so we want a total joint torque of 100 N mm
right_hand.elem( right_hand.rows()-1, 0) = 1.0e10;
//left-hand side of equality constraint
Matrix LeftHand( JTD.rows() + G.rows() + 1, D.cols() + numJoints);
LeftHand.setAllElements(0.0);
LeftHand.copySubMatrix(0, 0, JTD);
LeftHand.copySubMatrix(0, D.cols(), Matrix::NEGEYE(numJoints, numJoints) );
LeftHand.copySubMatrix(JTD.rows(), 0, G);
for (int i=0; i<numJoints; i++) {
LeftHand.elem( JTD.rows() + G.rows(), D.cols() + i) = 1.0;
}
//objective matrix
//matrix F padded with zeroes for tau
//will also serve as the left hand side of the inequality matrix
Matrix FO(F.rows(), F.cols() + numJoints);
FO.setAllElements(0.0);
FO.copySubMatrix(0, 0, F);
//summing matrix and objective matrix
Matrix FSum(1, F.rows());
FSum.setAllElements(1.0);
Matrix FObj(1, FO.cols());
matrixMultiply(FSum, FO, FObj);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta_tau.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta_tau.rows()));
//right-hand side of inequality matrix
Matrix inEqZeroes(FO.rows(), 1);
inEqZeroes.setAllElements(0.0);
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"left hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
FO.print(fp);
fprintf(fp,"Objective:\n");
FObj.print(fp);
fclose(fp);
// assembled system:
// minimize FObj * beta_tau subject to:
// LeftHand * beta_tau = right_hand
// FO * beta_tau <= inEqZeroes
// beta_tau >= 0
int result = LPSolver(FObj, LeftHand, right_hand, FO, inEqZeroes,
lowerBounds, upperBounds,
beta_tau, objVal);
beta.copySubBlock(0, 0, beta.rows(), 1, beta_tau, 0, 0);
tau.copySubBlock(0, 0, tau.rows(), 1, beta_tau, beta.rows(), 0);
return result;
}
void DisparityProc::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC1); // Choose 8 BIT Format, e.g. TYPE_8UC1, MONO8, ...
}
catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
/* --------------------------------------------------------------------------------------------
Select ROI & Inititialize Masks
-------------------------------------------------------------------------------------------*/
cv::Mat image = cv_ptr->image;
cv::Mat ROI(image, cv::Rect(x_min, y_min, width+1, height+1));
cv::Mat X = cv::Mat::zeros(height+1, width+1, CV_32FC1);
cv::Mat Y = cv::Mat::zeros(height+1, width+1, CV_32FC1);
// Create Meshgrid
cv::Range x_bound(0, width);
cv::Range y_bound(0, height);
meshgrid(x_bound, y_bound, X, Y);
// Initialize and compute Masks
const float PI = 3.14159265358979f;
Line line1(-2.3, 175, 2); // Bicaval
Line line2(0.8, 20, 1); // Short Axis
Line line3(0, 60, 1); // 4 Chamber
Ellipse FO(50, 50, 35, 33, -50 * PI/180); // Constructor(x0, y0, a, b, phi)
Rectangle needle(0, 0 ,10, 10); // Constructor(x0, y0, width, height)
cv::Mat mask_line1;
cv::Mat mask_line2;
cv::Mat mask_line3;
cv::Mat mask_FO;
cv::Mat mask_needle;
//std::cerr << "X: " << X.rows << "x" << X.cols << std::endl;
//std::cerr << "Y: " << Y.rows << "x" << Y.cols << std::endl;
line1.calc_mask(mask_line1, X, Y);
line2.calc_mask(mask_line2, X, Y);
line3.calc_mask(mask_line3, X, Y);
FO.calc_mask(mask_FO, X, Y);
// Auto Configuration
// ROI = compute_config(ROI);
/* --------------------------------------------------------------------------------------------
Compute Start & End Points of Cuts
-------------------------------------------------------------------------------------------*/
std::pair<int,int> bicaval_start;
std::pair<int,int> bicaval_stop;
std::pair<int,int> short_axis_start;
std::pair<int,int> short_axis_stop;
std::pair<int,int> four_chamber_start;
std::pair<int,int> four_chamber_stop;
start_end_coord(line1, mask_FO, X, Y, bicaval_start, bicaval_stop);
start_end_coord(line2, mask_FO, X, Y, short_axis_start, short_axis_stop);
start_end_coord(line3, mask_FO, X, Y, four_chamber_start, four_chamber_stop);
/* --------------------------------------------------------------------------------------------
Compute Update of Tenting Curve
Main Idea: Minimize difference between SPOT & OLD SPOT and introduce some kind of spacial delay
-------------------------------------------------------------------------------------------*/
// Check whether or not tenting even occurs
bool th_exceeded = tenting_ocurrs(ROI, mask_FO);
// Update Spot Location if Threshold is exceeded
if (th_exceeded) {
// Estimate current position of needle
find_needle_tip(ROI, mask_FO, needle);
needle_tip_ = needle.get_mid();
if (old_spot_init) {
// Compute whether or not intersection occurs - for each Projection
std::pair<int,int> tmp_proj1 = line1.projection(needle_tip_, bicaval_start);
std::pair<int,int> tmp_proj2 = line2.projection(needle_tip_, short_axis_start);
std::pair<int,int> tmp_proj3 = line3.projection(needle_tip_, four_chamber_start);
bool moving1 = needle_moving(old_proj_bicaval, tmp_proj1);
bool moving2 = needle_moving(old_proj_short_axis, tmp_proj2);
bool moving3 = needle_moving(old_proj_four_chamber, tmp_proj3);
// Update Bicaval
if (!moving1) {
proj_bicaval = old_proj_bicaval; // Stay Still
}
else {
minimize_with_constraint(needle, line1, 0); // Minimize Scalar Product
}
// Update Short Axis
if (!moving2) {
proj_short_axis = old_proj_short_axis; // Stay Still
}
else {
minimize_with_constraint(needle, line2, 1); // Minimize Scalar Product
}
// Update 4 Chamber
if (!moving3) {
proj_four_chamber = old_proj_four_chamber; // Stay Still
}
else {
minimize_with_constraint(needle, line3, 2); // Minimize Scalar Product
}
}
else {
// Old Spot not initialized -> Initialize
initialize_spot(needle,line1, bicaval_start, 0);
initialize_spot(needle,line2, short_axis_start, 1);
initialize_spot(needle,line3, four_chamber_start, 2);
old_spot_init = true;
}
}
else {
//std::cerr << "Threshold NOT exceeded..." << std::endl;
// Draw straight line
proj_bicaval = bicaval_start;
proj_short_axis = short_axis_start;
proj_four_chamber = four_chamber_start;
old_spot_init = false;
}
// Derive x/y _ points (new 1D coord sys) from spot
derive_xy_points(bicaval_start, bicaval_stop, ROI, 0);
derive_xy_points(short_axis_start, short_axis_stop, ROI, 1);
derive_xy_points(four_chamber_start, four_chamber_stop, ROI, 2);
// Update Old Spot
old_proj_bicaval = line1.projection(proj_bicaval, bicaval_start);
old_proj_short_axis = line2.projection(proj_short_axis, short_axis_start);
old_proj_four_chamber = line3.projection(proj_four_chamber, four_chamber_start);
// Print Values of Needle Location
/*
std::cerr << "Needle Location:" << std::endl;
int x_print = needle_tip_.first;
int y_print = needle_tip_.second;
int value = ROI.at<uint8_t>(y_print, x_print);
std::cerr << "(X,Y) = (" << x_print << ", " << y_print << ") : " << value << std::endl;
*/
// For Debugging -> Draw on FO
ROI = draw_on_FO (
ROI,
mask_FO,
mask_line1,
mask_line2,
mask_line3,
bicaval_start,
bicaval_stop,
short_axis_start,
short_axis_stop,
four_chamber_start,
four_chamber_stop,
needle );
/* --------------------------------------------------------------------------------------------
Interpoplate the two parabolas & evaluate on a fine mesh
-------------------------------------------------------------------------------------------*/
/*
* depricated...
*
Parabola left(x_points[0], y_points[0], x_points[1], y_points[1]);
Parabola right(x_points[2], y_points[2], x_points[1], y_points[1]);
int N_vis = 1000;
std::vector<float> x_vis(N_vis, 0);
std::vector<float> y_vis(N_vis, 0);
float h = (x_points[2]-x_points[0])/(N_vis-1);
for(unsigned int i=0; i<x_vis.size(); ++i) {
x_vis[i] = x_points[0] + i*h;
if (x_vis[i] < x_points[1])
y_vis[i] = left.eval_para(x_vis[i]);
else
y_vis[i] = right.eval_para(x_vis[i]);
}
*/
/* --------------------------------------------------------------------------------------------
Create Ros - Messages and publish data
-------------------------------------------------------------------------------------------*/
// Write ROI to published image
cv_ptr->image = ROI;
// Build Messages - BICAVAL
std_msgs::Float64MultiArray bicaval_x;
std_msgs::Float64MultiArray bicaval_y;
bicaval_x.layout.dim.push_back(std_msgs::MultiArrayDimension());
bicaval_x.layout.dim[0].size = x_pt_bicaval.size();
bicaval_x.layout.dim[0].stride = 1;
bicaval_x.layout.dim[0].label = "Bicaval_X_Val";
bicaval_x.data.clear();
bicaval_x.data.insert(bicaval_x.data.end(), x_pt_bicaval.begin(), x_pt_bicaval.end());
bicaval_y.layout.dim.push_back(std_msgs::MultiArrayDimension());
bicaval_y.layout.dim[0].size = y_pt_bicaval.size();
bicaval_y.layout.dim[0].stride = 1;
bicaval_y.layout.dim[0].label = "Bicaval_Y_Val";
bicaval_y.data.clear();
bicaval_y.data.insert(bicaval_y.data.end(), y_pt_bicaval.begin(), y_pt_bicaval.end());
// Build Messages - SHORT AXIS
std_msgs::Float64MultiArray short_axis_x;
std_msgs::Float64MultiArray short_axis_y;
short_axis_x.layout.dim.push_back(std_msgs::MultiArrayDimension());
short_axis_x.layout.dim[0].size = x_pt_short_axis.size();
short_axis_x.layout.dim[0].stride = 1;
short_axis_x.layout.dim[0].label = "Short_Axis_X_Val";
short_axis_x.data.clear();
short_axis_x.data.insert(short_axis_x.data.end(), x_pt_short_axis.begin(), x_pt_short_axis.end());
short_axis_y.layout.dim.push_back(std_msgs::MultiArrayDimension());
short_axis_y.layout.dim[0].size = y_pt_short_axis.size();
short_axis_y.layout.dim[0].stride = 1;
short_axis_y.layout.dim[0].label = "Short_Axis_Y_Val";
short_axis_y.data.clear();
short_axis_y.data.insert(short_axis_y.data.end(), y_pt_short_axis.begin(), y_pt_short_axis.end());
// Build Messages - 4 CHAMBER
std_msgs::Float64MultiArray four_chamber_x;
std_msgs::Float64MultiArray four_chamber_y;
four_chamber_x.layout.dim.push_back(std_msgs::MultiArrayDimension());
four_chamber_x.layout.dim[0].size = x_pt_four_chamber.size();
four_chamber_x.layout.dim[0].stride = 1;
four_chamber_x.layout.dim[0].label = "Four_Chamber_X_Val";
four_chamber_x.data.clear();
four_chamber_x.data.insert(four_chamber_x.data.end(), x_pt_four_chamber.begin(), x_pt_four_chamber.end());
four_chamber_y.layout.dim.push_back(std_msgs::MultiArrayDimension());
four_chamber_y.layout.dim[0].size = y_pt_four_chamber.size();
four_chamber_y.layout.dim[0].stride = 1;
four_chamber_y.layout.dim[0].label = "Four_Chamber_Y_Val";
four_chamber_y.data.clear();
four_chamber_y.data.insert(four_chamber_y.data.end(), y_pt_four_chamber.begin(), y_pt_four_chamber.end());
// Build Messages - NEEDLE LOCATION
std_msgs::Float64MultiArray needle_loc_msg;
bool punctured = true;
std::pair<float,float> tmp_ = FO.map_to_unit_circle(needle_tip_);
tmp_.second = -tmp_.second;
needle_loc_msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
needle_loc_msg.layout.dim[0].size = 3;
needle_loc_msg.layout.dim[0].stride = 1;
needle_loc_msg.layout.dim[0].label = "Needle_Location";
needle_loc_msg.data.clear();
needle_loc_msg.data.push_back(punctured);
needle_loc_msg.data.push_back(tmp_.first);
needle_loc_msg.data.push_back(tmp_.second);
// Publish
image_pub_.publish(cv_ptr->toImageMsg());
bicaval_x_pub.publish(bicaval_x);
bicaval_y_pub.publish(bicaval_y);
short_axis_x_pub.publish(short_axis_x);
short_axis_y_pub.publish(short_axis_y);
four_chamber_x_pub.publish(four_chamber_x);
four_chamber_y_pub.publish(four_chamber_y);
needle_loc_pub.publish(needle_loc_msg);
}
/*! Optimizes contact forces AND tendon route (defined by the insertion point
"arms" l_i) and some construction parameters (joint radius r and link
length l) to get 0 resultant wrench on object, with all contact forces
legally inside their friction cones.
The optimization objective is the equilibrium criterion, where joint
torques should be balanced by contact forces. However, joint torques
are expressed as a function of tendon force (which is assumed known since
there is just one tendon) and tendon and hand parameters.
JTD * beta = tau = f * (B * p + a)
p = [l r d]
To get rid of the free term, the relationship becomes:
[JTD -B -I] * [beta p a] = 0
with the variables in a fixed to their known values.
We also have to constrain the sum contact amplitudes so at least some force
is applied to the object. If we don't, it might find solutions where tendon
forces just cancel out in this particular configuration. This is not ideal,
as the value that we constrain it to is somewhat arbitrary. However, it should
be compensated since the equilibrium constraint is not hard, but rather the
optimization objective.
Alternatively, we can multiply the B and a matrices by the desired tendon
force which does the same thing a lot more elegantly.
Here, the constraints themselves are nothing but the force-closure criterion.
Therefore, they should be satisfiable anytime the grasp has f-c. The addition
is the equilibrium, which is hand specific, but is the objective rather than
a constraint.
The overall optimization problem has the form:
minimize
[beta p a] [JTD -B -I]^T [JTD -B -I] [beta p a]^t (joint equilibrium)
constrained by:
[G 0 0] [beta p a]^T = |0| (0 resultant wrench)
[S 0 0] |1| (some force applied on object)
[F 0 0] [beta p a]^T <= 0 (all forces inside cone)
beta >= 0 (legal contact forces)
p_min <= p <= p_max (limits on the parameters)
a = a_known (free term is known)
Return results: we report that the problem is unfeasible if either the
optimization itself if unfeasible OR the objective is not 0. However, if
the optimization is successful but with non-0 objective, we accumulate and
display contact forces as usual, and return the optimized parameters.
Codes: 0 is success; >0 means problem unfeasible; <0 means error in
computation
*/
int
McGripGrasp::tendonAndHandOptimization(Matrix *parameters, double &objValRet)
{
//use the pre-set list of contacts. This includes contacts on the palm, but
//not contacts with other objects or obstacles
std::list<Contact *> contacts;
contacts.insert(contacts.begin(), contactVec.begin(), contactVec.end());
//if there are no contacts we are done
if (contacts.empty()) { return 0; }
//retrieve all the joints of the robot. for now we get all the joints and in
//the specific order by chain. keep in mind that saved grasps do not have
//self-contacts so that will bias optimizations badly
//RECOMMENDED to use this for now only for grasps where both chains are
//touching the object and there are no self-contacts
std::list<Joint *> joints;
for (int c = 0; c < hand->getNumChains(); c++) {
std::list<Joint *> chainJoints = hand->getChain(c)->getJoints();
joints.insert(joints.end(), chainJoints.begin(), chainJoints.end());
}
//build the Jacobian and the other matrices that are needed.
//this is the same as in other equilibrium functions.
Matrix J(contactJacobian(joints, contacts));
Matrix D(Contact::frictionForceBlockMatrix(contacts));
Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));
//grasp map that relates contact amplitudes to object wrench G = S*R*D
Matrix G(graspMapMatrixFrictionEdges(R, D));
//matrix that relates contact forces to joint torques JTD = JTran * D
Matrix JTran(J.transposed());
Matrix JTD(JTran.cols(), D.cols());
matrixMultiply(JTran, D, JTD);
//get the routing equation matrices
Matrix *a, *negB;
static_cast<McGrip *>(hand)->getRoutingMatrices(&negB, &a);
negB->multiply(-1.0);
assert(JTD.cols() == negB->rows());
assert(JTD.cols() == a->rows());
//scale B and a by tendon force
double tendonForce = 1.0e6;
negB->multiply(tendonForce);
a->multiply(tendonForce);
//int numBetas = JTD.cols();
int numParameters = negB->cols();
//int numFree = a->rows();
//matrix of unknowns
Matrix p(JTD.cols() + negB->cols() + a->rows(), 1);
//optimization objective
Matrix Q(JTD.cols(), p.cols());
Q.copySubMatrix(0, 0, JTD);
Q.copySubMatrix(0, JTD.cols(), *negB);
Q.copySubMatrix(0, JTD.cols() + negB->cols(), Matrix::NEGEYE(a->rows(), a->rows()));
//friction matrix padded with zeroes
Matrix FO(Matrix::ZEROES<Matrix>(F.cols(), p.cols()));
FO.copySubMatrix(0, 0, F);
//equality constraint is just grasp map padded with zeroes
Matrix EqLeft(Matrix::ZEROES<Matrix>(G.cols(), p.cols()));
EqLeft.copySubMatrix(0, 0, G);
//and right hand side is just zeroes
Matrix eqRight(Matrix::ZEROES<Matrix>(G.cols(), 1));
/*
//let's add the summation to the equality constraint
Matrix EqLeft( Matrix::ZEROES<Matrix>(G.cols() + 1, p.cols()) );
EqLeft.copySubMatrix(0, 0, G);
Matrix sum(1, G.cols());
sum.setAllElements(1.0);
EqLeft.copySubMatrix(G.cols(), 0, sum);
//and the right side of the equality constraint
Matrix eqRight( Matrix::ZEROES<Matrix>(G.cols()+1, 1) );
//betas must sum to one
eqRight.elem(G.cols(), 0) = 1.0;
*/
//lower and upper bounds
Matrix lowerBounds(Matrix::MIN_VECTOR(p.cols()));
Matrix upperBounds(Matrix::MAX_VECTOR(p.cols()));
//betas are >= 0
for (int i = 0; i < JTD.cols(); i++) {
lowerBounds.elem(i, 0) = 0.0;
}
//l's have hard-coded upper and lower limits
for (int i = 0; i < 6; i++) {
lowerBounds.elem(JTD.cols() + i, 0) = -5.0;
upperBounds.elem(JTD.cols() + i, 0) = 5.0;
}
//use this if you already know the hand parameters and are just using this to check
//how close this grasp is to equilibrium
//tendon insertion points
lowerBounds.elem(JTD.cols() + 0, 0) = 5;
lowerBounds.elem(JTD.cols() + 1, 0) = 5;
lowerBounds.elem(JTD.cols() + 2, 0) = 1.65;
upperBounds.elem(JTD.cols() + 0, 0) = 5;
upperBounds.elem(JTD.cols() + 1, 0) = 5;
upperBounds.elem(JTD.cols() + 2, 0) = 1.65;
//--------------
lowerBounds.elem(JTD.cols() + 3, 0) = 5;
lowerBounds.elem(JTD.cols() + 4, 0) = 5;
lowerBounds.elem(JTD.cols() + 5, 0) = 1.65;
upperBounds.elem(JTD.cols() + 3, 0) = 5;
upperBounds.elem(JTD.cols() + 4, 0) = 5;
upperBounds.elem(JTD.cols() + 5, 0) = 1.65;
//r and d have their own hard-coded limits, fixed for now
lowerBounds.elem(JTD.cols() + 6, 0) = static_cast<McGrip *>(hand)->getJointRadius();
upperBounds.elem(JTD.cols() + 6, 0) = static_cast<McGrip *>(hand)->getJointRadius();
lowerBounds.elem(JTD.cols() + 7, 0) = static_cast<McGrip *>(hand)->getLinkLength();
upperBounds.elem(JTD.cols() + 7, 0) = static_cast<McGrip *>(hand)->getLinkLength();
//the "fake" variables in a are fixed
for (int i = 0; i < a->rows(); i++) {
lowerBounds.elem(JTD.cols() + 8 + i, 0) = a->elem(i, 0);
upperBounds.elem(JTD.cols() + 8 + i, 0) = a->elem(i, 0);
}
//we don't need the routing matrices any more
delete negB;
delete a;
//solve the whole thing
double objVal;
int result = factorizedQPSolver(Q,
EqLeft, eqRight,
FO, Matrix::ZEROES<Matrix>(FO.cols(), 1),
lowerBounds, upperBounds,
p, &objVal);
objValRet = objVal;
if (result) {
if (result > 0) {
DBGA("McGrip constr optimization: problem unfeasible");
} else {
DBGA("McGrip constr optimization: QP solver error");
}
return result;
}
DBGA("Construction optimization objective: " << objVal);
DBGP("Result:\n" << p);
//get the contact forces and the parameters; display contacts as usual
Matrix beta(JTD.cols(), 1);
beta.copySubBlock(0, 0, JTD.cols(), 1, p, 0, 0);
//scale betas by tendon foce
beta.multiply(1.0e7);
parameters->copySubBlock(0, 0, numParameters, 1, p, JTD.cols(), 0);
//retrieve contact wrenches in local contact coordinate systems
Matrix cWrenches(D.cols(), 1);
matrixMultiply(D, beta, cWrenches);
DBGP("Contact forces:\n " << cWrenches);
//compute object wrenches relative to object origin and expressed in world coordinates
Matrix objectWrenches(R.cols(), cWrenches.cols());
matrixMultiply(R, cWrenches, objectWrenches);
DBGP("Object wrenches:\n" << objectWrenches);
//display them on the contacts and accumulate them on the object
displayContactWrenches(&contacts, cWrenches);
accumulateAndDisplayObjectWrenches(&contacts, objectWrenches);
//we actually say the problem is also unfeasible if objective is not 0
//this is magnitude squared of contact wrench, where force units are N*1.0e6
//acceptable force is 1mN -> (1.0e3)^2 magnitude
if (objVal > 1.0e6) {
return 1;
}
return 0;
}
