//_________________________________________________________________________________________________
Constraint Inst::getConstraint(U_32 idx, U_32 memOpndMask, OpndSize size) const
{
Constraint c(OpndKind_Any);
if (idx < opndCount){
const U_32 * opndRoles = getOpndRoles();
const Constraint * constraints = getConstraints();
c = constraints[idx];
if ((opndRoles[idx] & OpndRole_Explicit) && (properties & Inst::Properties_MemoryOpndConditional)){
bool removeMemory = false;
if (memOpndMask == 0xFFFFFFFF){// Strong constraint requested
removeMemory = true;
} else if (memOpndMask != 0) {
for (unsigned j = 0; j < opndCount; j++) {
if (j != idx && ((1 << j) & memOpndMask) && (opndRoles[j] & OpndRole_Explicit)) {
if (getOpnd(j)->getConstraint(Opnd::ConstraintKind_Location).getKind() == OpndKind_Mem){
removeMemory = true;
break;
}
}
}
}
if (removeMemory)
c = Constraint((OpndKind)(c.getKind() &~ OpndKind_Mem), c.getSize(), c.getMask());
}
}else{
c = opnds[(idx - opndCount) / 4]->getMemOpndSubOpndConstraint((MemOpndSubOpndKind)((idx - opndCount) & 3));
}
return size==OpndSize_Null?c:c.getAliasConstraint(size);
}
DECLARE_EXPORT PyObject* SolverMRP::getattro(const Attribute& attr)
{
if (attr.isA(Tags::tag_constraints))
return PythonObject(getConstraints());
if (attr.isA(Tags::tag_autocommit))
return PythonObject(getAutocommit());
if (attr.isA(Tags::tag_userexit_flow))
return getUserExitFlow();
if (attr.isA(Tags::tag_userexit_demand))
return getUserExitDemand();
if (attr.isA(Tags::tag_userexit_buffer))
return getUserExitBuffer();
if (attr.isA(Tags::tag_userexit_resource))
return getUserExitResource();
if (attr.isA(Tags::tag_userexit_operation))
return getUserExitOperation();
if (attr.isA(Tags::tag_plantype))
return PythonObject(getPlanType());
// Less common parameters
if (attr.isA(tag_iterationthreshold))
return PythonObject(getIterationThreshold());
if (attr.isA(tag_iterationaccuracy))
return PythonObject(getIterationAccuracy());
if (attr.isA(tag_lazydelay))
return PythonObject(getLazyDelay());
if (attr.isA(tag_planSafetyStockFirst))
return PythonObject(getPlanSafetyStockFirst());
// Default parameters
return Solver::getattro(attr);
}
//_________________________________________________________________________________________________
void Inst::insertOpnd(U_32 idx, Opnd * opnd, U_32 roles)
{
assert(opndCount < allocatedOpndCount);
if (idx < defOpndCount)
roles |= OpndRole_Def;
if (roles & OpndRole_Def){
if (idx > defOpndCount)
idx = defOpndCount;
defOpndCount++;
}
if (idx > opndCount)
idx = opndCount;
U_32 * opndRoles = getOpndRoles();
Constraint * opndConstraints = getConstraints();
for (U_32 i = opndCount; i > idx; i--){
opnds[i] = opnds[i - 1];
opndRoles[i] = opndRoles[i - 1];
opndConstraints[i] = opndConstraints[i - 1];
}
opnds[idx] = opnd;
opndRoles[idx] = roles;
opndCount++;
}
static void updateConstraints(WMSplitView * sPtr)
{
W_SplitViewSubview *p;
int i, count;
count = _GetSubviewsCount();
for (i = 0; i < count; i++) {
p = _GetPSubviewStructAt(i);
getConstraints(sPtr, i, &(p->minSize), &(p->maxSize));
}
}
SpringLayoutConstraintsRefPtr SpringLayout::getConstraint(ComponentUnrecPtr TheComponent) const
{
FieldContainerMap::const_iterator
SearchItor(getConstraints().find(static_cast<FieldContainerMap::key_type>(TheComponent->getId())));
if(SearchItor == getConstraints().end())
{
SpringLayoutConstraintsUnrecPtr TempConstraints(SpringLayoutConstraintsBase::create());
SpringLayoutConstraintsRefPtr NewConstraints = applyDefaults(TheComponent, TempConstraints);
const_cast<SpringLayout*>(this)->editConstraints()[static_cast<FieldContainerMap::key_type>(TheComponent->getId())] = NewConstraints;
return NewConstraints;
}
else
{
return dynamic_pointer_cast<SpringLayoutConstraints>((*SearchItor).second);
}
}
//_________________________________________________________________________________________________
void Inst::makeNative(IRManager * irManager)
{
if (getForm()==Form_Native || hasKind(Kind_PseudoInst) )
return;
assert(opcodeGroup);
Node* bb = getNode();
U_32 * opndRoles = getOpndRoles();
Constraint * constraints = getConstraints();
I_32 defs[IRMaxNativeOpnds]={ -1, -1, -1, -1 };
for (U_32 i=0; i<opndCount; i++){
U_32 r = opndRoles[i];
if ((r & OpndRole_Explicit) == 0) continue;
U_32 extendedIdx = r >> 16;
U_32 nativeIdx = opcodeGroup->extendedToNativeMap[extendedIdx];
assert(nativeIdx < IRMaxNativeOpnds);
I_32 defNativeIdx = defs[nativeIdx];
if (defNativeIdx != -1){
assert(i >= defOpndCount);
opndRoles[defNativeIdx] |= OpndRole_Use;
if (bb && opnds[defNativeIdx] != opnds[i]){
Inst * moveInst=irManager->newCopySequence(Mnemonic_MOV, opnds[defNativeIdx], opnds[i]);
moveInst->insertBefore(this);
}
opnds[i] = NULL;
}else
defs[nativeIdx] = i;
}
U_32 packedOpnds = 0, explicitOpnds = 0;
for (U_32 i = 0, n = opndCount; i < n; i++){
if (opnds[i] == NULL) continue;
U_32 r = opndRoles[i];
if ((r & OpndRole_Explicit) != 0){
r = (r & 0xffff) | (explicitOpnds << 16);
explicitOpnds++;
}
if (i > packedOpnds){
opnds[packedOpnds] = opnds[i];
constraints[packedOpnds] = constraints[i];
opndRoles[packedOpnds] = r;
}
packedOpnds++;
}
opndCount=packedOpnds;
form = Form_Native;
}
void WMAddSplitViewSubview(WMSplitView * sPtr, WMView * subview)
{
int wasMapped, count;
W_SplitViewSubview *p;
CHECK_CLASS(sPtr, WC_SplitView);
p = (W_SplitViewSubview *) wmalloc(sizeof(W_SplitViewSubview));
if (!p)
return;
wasMapped = subview->flags.mapped;
if (wasMapped)
W_UnmapView(subview);
count = _GetSubviewsCount();
p->view = subview;
getConstraints(sPtr, count, &(p->minSize), &(p->maxSize));
if (sPtr->flags.vertical)
p->size = subview->size.width;
else
p->size = subview->size.height;
WMAddToArray(sPtr->subviews, p);
reparentView(sPtr, subview, 0);
/*
We should have something like that...
WMSetViewNotifySizeChanges(subview, True);
WMAddNotificationObserver(handleSubviewResized, sPtr,
WMViewSizeDidChangeNotification,
subview);
WMSetViewNotifyMoveChanges(subview, True);
WMAddNotificationObserver(handleSubviewResized, sPtr,
WMViewMoveDidChangeNotification,
subview);
*/
if (wasMapped) {
W_MapView(subview);
sPtr->flags.adjustOnPaint = 1;
paintSplitView(sPtr);
} else {
handleViewResized(sPtr, NULL);
}
}
//_________________________________________________________________________________________________
void Inst::fixOpndsForOpcodeGroup(IRManager * irManager)
{
U_32 handledExplicitOpnds = 0, handledImplicitOpnds = 0;
U_32 * roles = getOpndRoles();
Constraint * constraints = getConstraints();
Form f = getForm();
for (U_32 i=0, n=opndCount; i<n; i++){
if (roles[i] & Inst::OpndRole_Explicit){
U_32 idx, r;
if (f == Form_Native){
idx = handledExplicitOpnds;
r = Encoder::getOpndRoles(opcodeGroup->opndRoles, idx);
if ( (r & OpndRole_Def) != 0 && defOpndCount<=i)
defOpndCount = i + 1;
}else{
idx = opcodeGroup->extendedToNativeMap[handledExplicitOpnds];
r = i < defOpndCount ? OpndRole_Def : OpndRole_Use;
}
r |= Inst::OpndRole_Explicit | (handledExplicitOpnds << 16);
roles[i] = r;
constraints[i] = opcodeGroup->opndConstraints[idx];
handledExplicitOpnds++;
}else if (roles[i] & Inst::OpndRole_Implicit){
assert(handledImplicitOpnds < opcodeGroup->implicitOpndRoles.count);
assert(opnds[i]->getRegName() == opcodeGroup->implicitOpndRegNames[handledImplicitOpnds]);
handledImplicitOpnds++;
}
}
for (U_32 i = handledImplicitOpnds; i < opcodeGroup->implicitOpndRoles.count; i++){
RegName iorn = opcodeGroup->implicitOpndRegNames[i];
Opnd * implicitOpnd = irManager->getRegOpnd(iorn);
U_32 implicitOpndRoles =
Encoder::getOpndRoles(opcodeGroup->implicitOpndRoles, i) | Inst::OpndRole_Implicit;
if (implicitOpndRoles & OpndRole_Def){
insertOpnd(defOpndCount, implicitOpnd, implicitOpndRoles);
constraints[defOpndCount - 1] = Constraint(iorn);
}else{
insertOpnd(opndCount, implicitOpnd, implicitOpndRoles);
constraints[opndCount - 1] = Constraint(iorn);
}
}
}
void computeDB(const robot_model::RobotModelPtr &robot_model,
unsigned int ns, unsigned int ne)
{
planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(robot_model));
ompl_interface::OMPLInterface ompl_interface(robot_model);
moveit_msgs::Constraints c = getConstraints();
ompl_interface::ConstraintApproximationConstructionOptions opt;
opt.state_space_parameterization = "PoseModel";
opt.samples = ns;
opt.edges_per_sample = ne;
opt.explicit_motions = true;
opt.max_edge_length = 0.2;
opt.explicit_points_resolution = 0.05;
opt.max_explicit_points = 10;
ompl_interface.getConstraintsLibrary().addConstraintApproximation(c, "right_arm", ps, opt);
ompl_interface.getConstraintsLibrary().saveConstraintApproximations("~/constraints_approximation_database");
ROS_INFO("Done");
}
QJsonObject copySelected(const Scenario_T& sm, QObject* parent)
{
auto selectedConstraints = selectedElements(getConstraints(sm));
auto selectedEvents = selectedElements(getEvents(sm));
auto selectedTimeNodes = selectedElements(getTimeNodes(sm));
auto selectedStates = selectedElements(getStates(sm));
for(const ConstraintModel* constraint : selectedConstraints)
{
auto start_it = find_if(selectedStates,
[&] (const StateModel* state) { return state->id() == constraint->startState();});
if(start_it == selectedStates.end())
{
selectedStates.push_back(&sm.state(constraint->startState()));
}
auto end_it = find_if(selectedStates,
[&] (const StateModel* state) { return state->id() == constraint->endState();});
if(end_it == selectedStates.end())
{
selectedStates.push_back(&sm.state(constraint->endState()));
}
}
for(const StateModel* state : selectedStates)
{
auto ev_it = find_if(selectedEvents,
[&] (const EventModel* event) { return state->eventId() == event->id(); });
if(ev_it == selectedEvents.end())
{
selectedEvents.push_back(&sm.event(state->eventId()));
}
// If the previous or next constraint is not here, we set it to null in a copy.
}
for(const EventModel* event : selectedEvents)
{
auto tn_it = find_if(selectedTimeNodes,
[&] (const TimeNodeModel* tn) { return tn->id() == event->timeNode(); });
if(tn_it == selectedTimeNodes.end())
{
selectedTimeNodes.push_back(&sm.timeNode(event->timeNode()));
}
// If some events aren't there, we set them to null in a copy.
}
std::vector<TimeNodeModel*> copiedTimeNodes;
copiedTimeNodes.reserve(selectedTimeNodes.size());
for(const auto& tn : selectedTimeNodes)
{
auto clone_tn = new TimeNodeModel(*tn, tn->id(), parent);
auto events = clone_tn->events();
for(const auto& event : events)
{
auto absent = none_of(selectedEvents,
[&] (const EventModel* ev) { return ev->id() == event; });
if(absent)
clone_tn->removeEvent(event);
}
copiedTimeNodes.push_back(clone_tn);
}
std::vector<EventModel*> copiedEvents;
copiedEvents.reserve(selectedEvents.size());
for(const auto& ev : selectedEvents)
{
auto clone_ev = new EventModel(*ev, ev->id(), parent);
auto states = clone_ev->states();
for(const auto& state : states)
{
auto absent = none_of(selectedStates,
[&] (const StateModel* st) { return st->id() == state; });
if(absent)
clone_ev->removeState(state);
}
copiedEvents.push_back(clone_ev);
}
std::vector<StateModel*> copiedStates;
copiedStates.reserve(selectedStates.size());
auto& stack = iscore::IDocument::documentContext(*parent).commandStack;
for(const StateModel* st : selectedStates)
{
auto clone_st = new StateModel(*st, st->id(), stack, parent);
// NOTE : we must not serialize the state with their previous / next constraint
// since they will change once pasted and cause crash at the end of the ctor
// of StateModel. They are saved in the previous / next state of constraint anyway.
SetNoPreviousConstraint(*clone_st);
SetNoNextConstraint(*clone_st);
copiedStates.push_back(clone_st);
}
QJsonObject base;
base["Constraints"] = arrayToJson(selectedConstraints);
base["Events"] = arrayToJson(copiedEvents);
base["TimeNodes"] = arrayToJson(copiedTimeNodes);
base["States"] = arrayToJson(copiedStates);
for(auto elt : copiedTimeNodes)
delete elt;
for(auto elt : copiedEvents)
delete elt;
for(auto elt : copiedStates)
delete elt;
return base;
}
/*!
Fills in the constraint columns in the joint constraint matrix \a Nu . It also
computes the generalized position error between the two links or between the
one link and the world coordinates it should be fixed to. The errors are
copied into the corresponding elements of the \a eps vector. Nu^T * v = eps.
\a numBodies are the number of bodies in this dynamic island, and \a islandIndices
is a vector that maps a body to the index of that body within the vector
of bodies for this dynamic island. This allows us to compute the starting
row numbers for the links connected to this joint. \a ncn is a counter
that holds the current constraint number, thus the current column to use.
*/
void
DynJoint::buildConstraints(double *Nu,double *eps,int numBodies,
std::map<Body*,int> &islandIndices,int &ncn)
{
//compute the joint coordinate system as seen by both the previous and the
//next links
transf prevTrans = getPrevTrans() * prevLink->getTran();
transf nextTrans = getNextTrans() * nextLink->getTran();
//compute error
transf error = nextTrans.inverse() * prevTrans;
//we will use the joint coordinate system as reported by the previous link
//as reference
vec3 constrainedAxis[3];
constrainedAxis[0] = prevTrans.affine().row(0);
constrainedAxis[1] = prevTrans.affine().row(1);
constrainedAxis[2] = prevTrans.affine().row(2);
DBGP("Constrained axes:\n " << constrainedAxis[0] <<
"\n " << constrainedAxis[1] << "\n " << constrainedAxis[2]);
//set up cross product matrices for translations from joint location
//the the cog's of the links
vec3 prevCOG = (prevLink->getCoG() * prevLink->getTran())-position::ORIGIN;
vec3 nextCOG = (nextLink->getCoG() * nextLink->getTran())-position::ORIGIN;
//the constrained axes transformed for each of the links involved
//remember that prevTrans is considered the reference transform for this dyn joint
mat3 prevCross;
prevCross.setCrossProductMatrix( prevTrans.translation() - prevCOG );
prevCross *= prevTrans.affine().transpose();
mat3 nextCross;
nextCross.setCrossProductMatrix( nextTrans.translation() - nextCOG );
nextCross *= prevTrans.affine().transpose();
//find out which directions we are actually constraining
char constraints[6];
getConstraints(constraints);
//compute the indices into the big constraint matrix
assert(islandIndices[prevLink] >= 0);
assert(islandIndices[nextLink] >= 0);
int prevLinkRow = 6*islandIndices[prevLink];
int nextLinkRow = 6*islandIndices[nextLink];
//compute translation error in world coordinate system
vec3 errorVec = nextTrans.translation() - prevTrans.translation();
DBGP("Translation error: " << errorVec);
//constrain translations
for (int c=0; c<3; c++) {
if (!constraints[c]) continue;
Nu[(ncn)*6*numBodies + prevLinkRow] -= constrainedAxis[c][0];
Nu[(ncn)*6*numBodies + prevLinkRow+1] -= constrainedAxis[c][1];
Nu[(ncn)*6*numBodies + prevLinkRow+2] -= constrainedAxis[c][2];
Nu[(ncn)*6*numBodies + prevLinkRow+3] -= prevCross[3*c];
Nu[(ncn)*6*numBodies + prevLinkRow+4] -= prevCross[3*c+1];
Nu[(ncn)*6*numBodies + prevLinkRow+5] -= prevCross[3*c+2];
Nu[(ncn)*6*numBodies + nextLinkRow] += constrainedAxis[c][0];
Nu[(ncn)*6*numBodies + nextLinkRow+1] += constrainedAxis[c][1];
Nu[(ncn)*6*numBodies + nextLinkRow+2] += constrainedAxis[c][2];
Nu[(ncn)*6*numBodies + nextLinkRow+3] += nextCross[3*c];
Nu[(ncn)*6*numBodies + nextLinkRow+4] += nextCross[3*c+1];
Nu[(ncn)*6*numBodies + nextLinkRow+5] += nextCross[3*c+2];
eps[ncn] = -(errorVec % constrainedAxis[c]);
ncn++;
}
// constrain rotations
double alpha;
error.rotation().ToAngleAxis(alpha, errorVec);
errorVec = - errorVec * alpha;
DBGP("Rotation error: " << errorVec);
for (int c=0; c<3; c++) {
if (!constraints[3+c]) continue;
Nu[(ncn)*6*numBodies + prevLinkRow+3] -= constrainedAxis[c][0];
Nu[(ncn)*6*numBodies + prevLinkRow+4] -= constrainedAxis[c][1];
Nu[(ncn)*6*numBodies + prevLinkRow+5] -= constrainedAxis[c][2];
Nu[(ncn)*6*numBodies + nextLinkRow+3] += constrainedAxis[c][0];
Nu[(ncn)*6*numBodies + nextLinkRow+4] += constrainedAxis[c][1];
Nu[(ncn)*6*numBodies + nextLinkRow+5] += constrainedAxis[c][2];
eps[ncn] = -(errorVec % constrainedAxis[c]);
ncn++;
}
}
/** problem reading method of reader */
static
SCIP_DECL_READERREAD(readerReadCip)
{ /*lint --e{715}*/
CIPINPUT cipinput;
SCIP_Real objscale;
SCIP_Real objoffset;
SCIP_Bool dynamicconss;
SCIP_Bool dynamiccols;
SCIP_Bool dynamicrows;
SCIP_Bool initialvar;
SCIP_Bool removablevar;
SCIP_Bool initialcons;
SCIP_Bool removablecons;
if( NULL == (cipinput.file = SCIPfopen(filename, "r")) )
{
SCIPerrorMessage("cannot open file <%s> for reading\n", filename);
SCIPprintSysError(filename);
return SCIP_NOFILE;
}
cipinput.len = 131071;
SCIP_CALL( SCIPallocBufferArray(scip, &(cipinput.strbuf), cipinput.len) );
cipinput.linenumber = 0;
cipinput.section = CIP_START;
cipinput.haserror = FALSE;
cipinput.endfile = FALSE;
cipinput.readingsize = 65535;
SCIP_CALL( SCIPcreateProb(scip, filename, NULL, NULL, NULL, NULL, NULL, NULL, NULL) );
SCIP_CALL( SCIPgetBoolParam(scip, "reading/"READER_NAME"/dynamiccols", &dynamiccols) );
SCIP_CALL( SCIPgetBoolParam(scip, "reading/"READER_NAME"/dynamicconss", &dynamicconss) );
SCIP_CALL( SCIPgetBoolParam(scip, "reading/"READER_NAME"/dynamicrows", &dynamicrows) );
initialvar = !dynamiccols;
removablevar = dynamiccols;
initialcons = !dynamicrows;
removablecons = dynamicrows;
objscale = 1.0;
objoffset = 0.0;
while( cipinput.section != CIP_END && !cipinput.haserror )
{
/* get next input string */
SCIP_CALL( getInputString(scip, &cipinput) );
if( cipinput.endfile )
break;
switch( cipinput.section )
{
case CIP_START:
SCIP_CALL( getStart(scip, &cipinput) );
break;
case CIP_STATISTIC:
SCIP_CALL( getStatistic(scip, &cipinput) );
break;
case CIP_OBJECTIVE:
SCIP_CALL( getObjective(scip, &cipinput, &objscale, &objoffset) );
break;
case CIP_VARS:
SCIP_CALL( getVariable(scip, &cipinput, initialvar, removablevar, objscale) );
break;
case CIP_FIXEDVARS:
SCIP_CALL( getFixedVariables(scip, &cipinput) );
break;
case CIP_CONSTRAINTS:
SCIP_CALL( getConstraints(scip, &cipinput, initialcons, dynamicconss, removablecons) );
break;
default:
SCIPerrorMessage("invalid CIP state\n");
SCIPABORT();
} /*lint !e788*/
}
if( !SCIPisZero(scip, objoffset) && !cipinput.haserror )
{
SCIP_VAR* objoffsetvar;
objoffset *= objscale;
SCIP_CALL( SCIPcreateVar(scip, &objoffsetvar, "objoffset", objoffset, objoffset, 1.0, SCIP_VARTYPE_CONTINUOUS,
TRUE, TRUE, NULL, NULL, NULL, NULL, NULL) );
SCIP_CALL( SCIPaddVar(scip, objoffsetvar) );
SCIP_CALL( SCIPreleaseVar(scip, &objoffsetvar) );
SCIPdebugMessage("added variables <objoffset> for objective offset of <%g>\n", objoffset);
}
/* close file stream */
SCIPfclose(cipinput.file);
if( cipinput.section != CIP_END && !cipinput.haserror )
{
SCIPerrorMessage("unexpected EOF\n");
}
SCIPfreeBufferArray(scip, &cipinput.strbuf);
if( cipinput.haserror )
return SCIP_READERROR;
/* successfully parsed cip format */
*result = SCIP_SUCCESS;
return SCIP_OKAY;
}