REAL totalCurrent(REAL time, struct State *S, struct State *Sn, REAL ht, struct Cpar *C, struct Is *I, struct Js *J, struct Buf *B, struct Buf *Bn)
{
REAL IKtot, INatot, ICltot, ICatot;
srflux(S, Sn, ht, C, J);
cabuf(S, Sn, ht, C, J, B, Bn);
ical(S, Sn, ht, C, I);
ibgca(S, Sn, ht, C, I);
ipk(S, Sn, ht, C, I);
ina(S, Sn, ht, C, I);
ibgna(S, Sn, ht, C, I);
incx(S, Sn, ht, C, I);
inak(S, Sn, ht, C, I);
ito(S, Sn, ht, C, I);
ikr(time, S, Sn, ht, C, I);
iks(S, Sn, ht, C, I);
iki(time,S, Sn, ht, C, I);
ikp(S, Sn, ht, C, I);
iclca(S, Sn, ht, C, I);
ibgcl(S, Sn, ht, C, I);
caconc(S, Sn, ht, C, J, I);
naconc(S, Sn, ht, C, J, I);
INatot = (I->INaJunc + I->INaSl) + (I->INaBkJunc + I->INaBkSl) + 3.0 * (I->IncxJunc + I->IncxSl) + 3.0 * (I->INaKJunc + I->INaKSl) + (I->ICaNaJunc + I->ICaNaSl);
IKtot = (I->ItoSlow + I->ItoFast) + (I->Ikr + I->Iks) + I->Iki - 2.0 * (I->INaKJunc + I->INaKSl) + I->Ikp + I->ICaK;
ICltot = I->IClCa + I->IBgCl;
ICatot = I->ICaTotJunc + I->ICaTotSl;
return INatot + IKtot + ICltot + ICatot;
}/** totalCurrent **/
/*b_prime copies the arrays pointed to by *x and *y to std::vectors iks, yps of type
Eigen::Vector3cd, respectively
After this the Multiplication of the Laplace takes place. Result is stored
in yps, which then is written to the array at *y again. */
static void b_prime(int nx,const PetscScalar *x,PetscScalar *y) {
const int V3 = pars -> get_int("V3");
const double LAM_L = pars -> get_float("lambda_l");
//define vectors
std::vector<Eigen::Vector3cd> iks(V3, Eigen::Vector3cd::Zero());
std::vector<Eigen::Vector3cd> yps(V3, Eigen::Vector3cd::Zero());
#pragma omp parallel
{
Eigen::Vector3cd tmp_x, tmp_y;
//copy read in vectors x and y to vectors of 3cd vectors
#pragma omp for
for(unsigned i = 0; i < V3; ++i) {
tmp_x << x[3*i], x[3*i+1], x[3*i+2];
tmp_y << y[3*i], y[3*i+1], y[3*i+2];
iks[i] = tmp_x;
yps[i] = tmp_y;
}
//constants used often: c := -2/lambda_L
//a := -1.
register const double c = -2./(LAM_L);
register const double a = -1.;
#pragma omp for
for ( int k = 0; k < V3; ++k) {
yps[k] = c * ( (ts -> get_gauge(k,0)) * iks.at( lookup -> get_up(k,0) )
+ ( (ts -> get_gauge( lookup -> get_dn(k,0), 0)).adjoint())
* iks.at( lookup -> get_dn(k,0) )
+ (ts -> get_gauge(k,1)) * iks.at( lookup -> get_up(k,1) )
+ (ts -> get_gauge( lookup -> get_dn(k,1),1).adjoint())
* iks.at( lookup -> get_dn(k,1) )
+ ts -> get_gauge(k,2) * iks.at( lookup -> get_up(k,2) )
+ (ts -> get_gauge( lookup -> get_dn(k, 2), 2).adjoint())
* iks.at( lookup -> get_dn(k,2) )
- 6.0 * (iks.at(k))) + a * (iks.at(k));
}
//copy vectors back to Petsc-arrays
#pragma omp for
for(unsigned j = 0; j < V3; ++j) {
y[3*j] = (yps[j])(0);
y[3*j+1] = (yps[j])(1);
y[3*j+2] = (yps[j])(2);
}
}
}
TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSampler)
{
robot_state::RobotState ks(kmodel);
ks.setToDefaultValues();
ks.update();
robot_state::RobotState ks_const(kmodel);
ks_const.setToDefaultValues();
ks_const.update();
robot_state::Transforms &tf = ps->getTransformsNonConst();
kinematic_constraints::OrientationConstraint oc(kmodel);
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = ocm.link_name;
ocm.orientation.x = 0.5;
ocm.orientation.y = 0.5;
ocm.orientation.z = 0.5;
ocm.orientation.w = 0.5;
ocm.absolute_x_axis_tolerance = 0.01;
ocm.absolute_y_axis_tolerance = 0.01;
ocm.absolute_z_axis_tolerance = 0.01;
ocm.weight = 1.0;
EXPECT_TRUE(oc.configure(ocm, tf));
bool p1 = oc.decide(ks).satisfied;
EXPECT_FALSE(p1);
ocm.header.frame_id = kmodel->getModelFrame();
EXPECT_TRUE(oc.configure(ocm, tf));
constraint_samplers::IKConstraintSampler iks(ps, "right_arm");
EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(oc)));
for (int t = 0 ; t < 100; ++t)
{
ks.update();
EXPECT_TRUE(iks.sample(ks, ks_const, 100));
EXPECT_TRUE(oc.decide(ks).satisfied);
}
}
constraint_samplers::ConstraintSamplerPtr constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planning_scene::PlanningSceneConstPtr &scene,
const std::string &group_name,
const moveit_msgs::Constraints &constr)
{
const robot_model::JointModelGroup *jmg = scene->getRobotModel()->getJointModelGroup(group_name);
if (!jmg)
return constraint_samplers::ConstraintSamplerPtr();
std::stringstream ss; ss << constr;
logDebug("Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n", jmg->getName().c_str(), ss.str().c_str());
ConstraintSamplerPtr joint_sampler;
// if there are joint constraints, we could possibly get a sampler from those
if (!constr.joint_constraints.empty())
{
logDebug("There are joint constraints specified. Attempting to construct a JointConstraintSampler for group '%s'", jmg->getName().c_str());
std::map<std::string, bool> joint_coverage;
for(std::size_t i = 0; i < jmg->getVariableNames().size() ; ++i)
joint_coverage[jmg->getVariableNames()[i]] = false;
// construct the constraints
std::vector<kinematic_constraints::JointConstraint> jc;
for (std::size_t i = 0 ; i < constr.joint_constraints.size() ; ++i)
{
kinematic_constraints::JointConstraint j(scene->getRobotModel(), scene->getTransforms());
if (j.configure(constr.joint_constraints[i]))
{
if (joint_coverage.find(j.getJointVariableName()) != joint_coverage.end())
{
joint_coverage[j.getJointVariableName()] = true;
jc.push_back(j);
}
}
}
bool full_coverage = true;
for (std::map<std::string, bool>::iterator it = joint_coverage.begin(); it != joint_coverage.end(); ++it)
if (!it->second)
{
full_coverage = false;
break;
}
// if we have constrained every joint, then we just use a sampler using these constraints
if (full_coverage)
{
boost::shared_ptr<JointConstraintSampler> sampler(new JointConstraintSampler(scene, jmg->getName()));
if (sampler->configure(jc))
{
logDebug("Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
return sampler;
}
}
else
// if a smaller set of joints has been specified, keep the constraint sampler around, but use it only if no IK sampler has been specified.
if (!jc.empty())
{
boost::shared_ptr<JointConstraintSampler> sampler(new JointConstraintSampler(scene, jmg->getName()));
if (sampler->configure(jc))
{
logDebug("Temporary sampler satisfying joint constraints for group '%s' allocated. Looking for different types of constraints before returning though.", jmg->getName().c_str());
joint_sampler = sampler;
}
}
}
std::vector<ConstraintSamplerPtr> samplers;
if (joint_sampler)
samplers.push_back(joint_sampler);
// read the ik allocators, if any
robot_model::SolverAllocatorFn ik_alloc = jmg->getSolverAllocators().first;
std::map<const robot_model::JointModelGroup*, robot_model::SolverAllocatorFn> ik_subgroup_alloc = jmg->getSolverAllocators().second;
// if we have a means of computing complete states for the group using IK, then we try to see if any IK constraints should be used
if (ik_alloc)
{
logDebug("There is an IK allocator for '%s'. Checking for corresponding position and/or orientation constraints", jmg->getName().c_str());
// keep track of which links we constrained
std::map<std::string, boost::shared_ptr<IKConstraintSampler> > usedL;
// if we have position and/or orientation constraints on links that we can perform IK for,
// we will use a sampleable goal region that employs IK to sample goals;
// if there are multiple constraints for the same link, we keep the one with the smallest
// volume for sampling
for (std::size_t p = 0 ; p < constr.position_constraints.size() ; ++p)
for (std::size_t o = 0 ; o < constr.orientation_constraints.size() ; ++o)
if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
{
boost::shared_ptr<kinematic_constraints::PositionConstraint> pc(new kinematic_constraints::PositionConstraint(scene->getRobotModel(), scene->getTransforms()));
boost::shared_ptr<kinematic_constraints::OrientationConstraint> oc(new kinematic_constraints::OrientationConstraint(scene->getRobotModel(), scene->getTransforms()));
if (pc->configure(constr.position_constraints[p]) && oc->configure(constr.orientation_constraints[o]))
{
boost::shared_ptr<IKConstraintSampler> iks(new IKConstraintSampler(scene, jmg->getName()));
if(iks->configure(IKSamplingPose(pc, oc))) {
bool use = true;
if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
use = false;
if (use)
{
usedL[constr.position_constraints[p].link_name] = iks;
logDebug("Allocated an IK-based sampler for group '%s' satisfying position and orientation constraints on link '%s'",
jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
}
}
}
}
// keep track of links constrained with a full pose
std::map<std::string, boost::shared_ptr<IKConstraintSampler> > usedL_fullPose = usedL;
for (std::size_t p = 0 ; p < constr.position_constraints.size() ; ++p)
{
// if we are constraining this link with a full pose, we do not attempt to constrain it with a position constraint only
if (usedL_fullPose.find(constr.position_constraints[p].link_name) != usedL_fullPose.end())
continue;
boost::shared_ptr<kinematic_constraints::PositionConstraint> pc(new kinematic_constraints::PositionConstraint(scene->getRobotModel(), scene->getTransforms()));
if (pc->configure(constr.position_constraints[p]))
{
boost::shared_ptr<IKConstraintSampler> iks(new IKConstraintSampler(scene, jmg->getName()));
if(iks->configure(IKSamplingPose(pc)))
{
bool use = true;
if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
use = false;
if (use)
{
usedL[constr.position_constraints[p].link_name] = iks;
logDebug("Allocated an IK-based sampler for group '%s' satisfying position constraints on link '%s'",
jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
}
}
}
}
for (std::size_t o = 0 ; o < constr.orientation_constraints.size() ; ++o)
{
// if we are constraining this link with a full pose, we do not attempt to constrain it with an orientation constraint only
if (usedL_fullPose.find(constr.orientation_constraints[o].link_name) != usedL_fullPose.end())
continue;
boost::shared_ptr<kinematic_constraints::OrientationConstraint> oc(new kinematic_constraints::OrientationConstraint(scene->getRobotModel(), scene->getTransforms()));
if (oc->configure(constr.orientation_constraints[o]))
{
boost::shared_ptr<IKConstraintSampler> iks(new IKConstraintSampler(scene, jmg->getName()));
if(iks->configure(IKSamplingPose(oc)))
{
bool use = true;
if (usedL.find(constr.orientation_constraints[o].link_name) != usedL.end())
if (usedL[constr.orientation_constraints[o].link_name]->getSamplingVolume() < iks->getSamplingVolume())
use = false;
if (use)
{
usedL[constr.orientation_constraints[o].link_name] = iks;
logDebug("Allocated an IK-based sampler for group '%s' satisfying orientation constraints on link '%s'",
jmg->getName().c_str(), constr.orientation_constraints[o].link_name.c_str());
}
}
}
}
if (usedL.size() == 1)
{
if (samplers.empty())
return usedL.begin()->second;
else
{
samplers.push_back(usedL.begin()->second);
return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
}
}
else
if (usedL.size() > 1)
{
logDebug("Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume", jmg->getName().c_str());
// find the sampler with the smallest sampling volume; delete the rest
boost::shared_ptr<IKConstraintSampler> iks = usedL.begin()->second;
double msv = iks->getSamplingVolume();
for (std::map<std::string, boost::shared_ptr<IKConstraintSampler> >::const_iterator it = ++usedL.begin() ; it != usedL.end() ; ++it)
{
double v = it->second->getSamplingVolume();
if (v < msv)
{
iks = it->second;
msv = v;
}
}
if (samplers.empty())
{
return iks;
}
else
{
samplers.push_back(iks);
return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
}
}
}
// if we got to this point, we have not decided on a sampler.
// we now check to see if we can use samplers from subgroups
if (!ik_subgroup_alloc.empty())
{
logDebug("There are IK allocators for subgroups of group '%s'. Checking for corresponding position and/or orientation constraints", jmg->getName().c_str());
bool some_sampler_valid = false;
std::set<std::size_t> usedP, usedO;
for (std::map<const robot_model::JointModelGroup*, robot_model::SolverAllocatorFn>::const_iterator it = ik_subgroup_alloc.begin() ; it != ik_subgroup_alloc.end() ; ++it)
{
// construct a sub-set of constraints that operate on the sub-group for which we have an IK allocator
moveit_msgs::Constraints sub_constr;
for (std::size_t p = 0 ; p < constr.position_constraints.size() ; ++p)
if (it->first->hasLinkModel(constr.position_constraints[p].link_name))
if (usedP.find(p) == usedP.end())
{
sub_constr.position_constraints.push_back(constr.position_constraints[p]);
usedP.insert(p);
}
for (std::size_t o = 0 ; o < constr.orientation_constraints.size() ; ++o)
if (it->first->hasLinkModel(constr.orientation_constraints[o].link_name))
if (usedO.find(o) == usedO.end())
{
sub_constr.orientation_constraints.push_back(constr.orientation_constraints[o]);
usedO.insert(o);
}
// if some matching constraints were found, construct the allocator
if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty())
{
logDebug("Attempting to construct a sampler for the '%s' subgroup of '%s'", it->first->getName().c_str(), jmg->getName().c_str());
ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr);
if (cs)
{
logDebug("Constructed a sampler for the joints corresponding to group '%s', but part of group '%s'",
it->first->getName().c_str(), jmg->getName().c_str());
some_sampler_valid = true;
samplers.push_back(cs);
}
}
}
if (some_sampler_valid)
{
logDebug("Constructing sampler for group '%s' as a union of %u samplers", jmg->getName().c_str(), (unsigned int)samplers.size());
return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
}
}
//if we've gotten here, just return joint sampler
if (joint_sampler)
{
logDebug("Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
return joint_sampler;
}
logDebug("No constraints sampler allocated for group '%s'", jmg->getName().c_str());
return ConstraintSamplerPtr();
}
/*tv copies the arrays pointed to by *x and *y to std::vectors iks, yps of type
Eigen::Vector3cd, respectively
After this the Multiplication of the Laplace takes place. Result is stored
in yps, which then is written to the array at *y again. */
static void tv2(int nx,const PetscScalar *x,PetscScalar *y) {
const int V3 = pars -> get_int("V3");
const double LAM_L = pars -> get_float("lambda_l");
const double LAM_C = pars -> get_float("lambda_c");
//define vectors
std::vector<Eigen::Vector3cd> iks(V3, Eigen::Vector3cd::Zero());
std::vector<Eigen::Vector3cd> yps(V3, Eigen::Vector3cd::Zero());
//iks.clear();
//yps.clear();
//Eigen::Vector3cd tmp_x, tmp_y;
omp_set_num_threads(pars -> get_int("OMP_THRDS"));
//std::cout << "Calculating with " << omp_get_num_threads() << " threads" << std::endl;
#pragma omp parallel
{
Eigen::Vector3cd tmp_x, tmp_y;
//copy read in vectors x and y to vectors of 3cd vectors
#pragma omp for
for(unsigned i = 0; i < V3; ++i) {
tmp_x << x[3*i], x[3*i+1], x[3*i+2];
tmp_y << y[3*i], y[3*i+1], y[3*i+2];
iks[i] = tmp_x;
yps[i] = tmp_y;
}
// for (unsigned i = 0; (i+3) <= 3*V3 ; i += 3){
// //Eigen::Vector3cd tmp_x;
// //Eigen::Vector3cd tmp_y;
// tmp_x << x[i], x[i+1], x[i+2];
// tmp_y << y[i], y[i+1], y[i+2];
// iks.push_back(tmp_x);
// yps.push_back(tmp_y);
// }
//constants used often: c := 2/ (lambda_L - lambda_C)
//a := 1+2*lambda_C / (lambda_L - lambda_C)
register const double c = 2./(LAM_L - LAM_C);
register const double a = 1 + c * LAM_C;
//these disable chebyshev acceleration:
//register const double c = 1;
//register const double a = 0;
//Laplace times vector in terms of Eigen::3cd
//for ( int k = 0; k < V3; ++k ) yps.at(k) = Eigen::Vector3cd::Zero();
#pragma omp for
// {
for ( int k = 0; k < V3; ++k) {
/*if (k == 0) {
yps.at(k) = -(U[k][0]*iks.at( up_3d[k][0] ) + (U[ down_3d[k][0] ][0].adjoint())
* iks.at( down_3d[k][0] ) + U[k][1] * iks.at( up_3d[k][1] )
+ (U[ down_3d[k][1] ][1].adjoint()) * iks.at( down_3d[k][1] )
+ U[k][2] * iks.at( up_3d[k][2] )
+ (U[ down_3d[k][2] ][2].adjoint()) * iks.at( down_3d[k][2] )
- 200.0 * (iks.at(k)));
}*/
//else {
yps[k] = c * ( (ts -> get_gauge(k,0)) * iks.at( lookup -> get_up(k,0) )
+ ( (ts -> get_gauge( lookup -> get_dn(k,0), 0)).adjoint())
* iks.at( lookup -> get_dn(k,0) )
+ (ts -> get_gauge(k,1)) * iks.at( lookup -> get_up(k,1) )
+ (ts -> get_gauge( lookup -> get_dn(k,1),1).adjoint())
* iks.at( lookup -> get_dn(k,1) )
+ ts -> get_gauge(k,2) * iks.at( lookup -> get_up(k,2) )
+ (ts -> get_gauge( lookup -> get_dn(k, 2), 2).adjoint())
* iks.at( lookup -> get_dn(k,2) )
- 6.0 * (iks.at(k))) + a * (iks.at(k));
// yps.at(k) = c * (eigen_timeslice[k][0]*iks.at( up_3d[k][0] ) + (eigen_timeslice[ down_3d[k][0] ][0].adjoint())
// * iks.at( down_3d[k][0] ) + eigen_timeslice[k][1] * iks.at( up_3d[k][1] )
// + (eigen_timeslice[ down_3d[k][1] ][1].adjoint()) * iks.at( down_3d[k][1] )
// + eigen_timeslice[k][2] * iks.at( up_3d[k][2] )
// + (eigen_timeslice[ down_3d[k][2] ][2].adjoint()) * iks.at( down_3d[k][2] )
// - 6.0 * (iks.at(k))) + a * (iks.at(k));
// std::cout << U[k][0] << " " << down_3d[k][0] << " " << up_3d[k][1] << " " << down_3d[k][1] << " " << up_3d[k][2] << " " << down_3d[k][2] << std::endl;
//}
}
// }
//copy vectors back to Petsc-arrays
#pragma omp for
for(unsigned j = 0; j < V3; ++j) {
y[3*j] = (yps[j])(0);
y[3*j+1] = (yps[j])(1);
y[3*j+2] = (yps[j])(2);
}
}
// int k = 0;
// for (int j = 0; j < V3; j++) {
// y[k] = (yps.at(j))(0);
// y[k+1] = (yps.at(j))(1);
// y[k+2] = (yps.at(j))(2);
// k += 3;
// }
}
TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSimple)
{
robot_state::Transforms &tf = ps->getTransformsNonConst();
kinematic_constraints::PositionConstraint pc(kmodel);
moveit_msgs::PositionConstraint pcm;
pcm.link_name = "l_wrist_roll_link";
pcm.target_point_offset.x = 0;
pcm.target_point_offset.y = 0;
pcm.target_point_offset.z = 0;
pcm.constraint_region.primitives.resize(1);
pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
pcm.constraint_region.primitives[0].dimensions.resize(1);
pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
pcm.constraint_region.primitive_poses.resize(1);
pcm.constraint_region.primitive_poses[0].position.x = 0.55;
pcm.constraint_region.primitive_poses[0].position.y = 0.2;
pcm.constraint_region.primitive_poses[0].position.z = 1.25;
pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
pcm.weight = 1.0;
EXPECT_FALSE(pc.configure(pcm, tf));
constraint_samplers::IKConstraintSampler ik_bad(ps, "l_arm");
EXPECT_FALSE(ik_bad.isValid());
constraint_samplers::IKConstraintSampler iks(ps, "left_arm");
EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose()));
EXPECT_FALSE(iks.isValid());
EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
pcm.header.frame_id = kmodel->getModelFrame();
EXPECT_TRUE(pc.configure(pcm, tf));
EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
//ik link not in this group
constraint_samplers::IKConstraintSampler ik_bad_2(ps, "right_arm");
EXPECT_FALSE(ik_bad_2.configure(constraint_samplers::IKSamplingPose(pc)));
EXPECT_FALSE(ik_bad_2.isValid());
//not the ik link
pcm.link_name = "l_shoulder_pan_link";
EXPECT_TRUE(pc.configure(pcm, tf));
EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
//solver for base doesn't cover group
constraint_samplers::IKConstraintSampler ik_base(ps, "base");
pcm.link_name = "l_wrist_roll_link";
EXPECT_TRUE(pc.configure(pcm, tf));
EXPECT_FALSE(ik_base.configure(constraint_samplers::IKSamplingPose(pc)));
EXPECT_FALSE(ik_base.isValid());
//shouldn't work as no direct constraint solver
constraint_samplers::IKConstraintSampler ik_arms(ps, "arms");
EXPECT_FALSE(iks.isValid());
}
