Exemple #1
0
void JamCoinCreator::createJamCoins(unsigned int count, unsigned int length)
{
  uint64_t smallest = 1;
  for (unsigned int i = 0; i < length-1; i++) smallest *= 10;
  smallest++;
  //std::cout << smallest << "," << max_value << std::endl;
  for (smallest; smallest < max_value && jamcoins.size() < count; smallest = getNextValidJamCoin(smallest)) {
    std::vector<uint64_t> proofs;
    JamCoin jc(smallest);
    std::cout << smallest << std::endl;
    for (unsigned int j = 2; j <= 10; j++) {
      uint64_t calc_value = jc.interpretValueInBase(j);
      /*if (calc_value%2 == 0) proofs.push_back(2);
      else if (composites[(calc_value-1)/2] != 0) proofs.push_back(composites[(calc_value-1)/2]);
      else break;*/ // jamcoin is prime
      uint64_t prime_factor;
      if (bruteForcePrime(calc_value, prime_factor)) break;
      else proofs.push_back(prime_factor);
    }
    if (proofs.size() == 9) {
      jc.proofs = std::vector<uint64_t>(proofs);
      jc.valid = true;
      jamcoins.push_back(jc);
    }
  }
}
int main(int argc, char** argv)
{
    QCoreApplication app(argc, argv);

    QHostAddress dest_addr;
    unsigned int fft_send_interval = 1024;
    if (argc > 1) {
        QHostInfo host_info = QHostInfo::fromName(argv[1]);
        if (host_info.error() != QHostInfo::NoError
            || host_info.addresses().empty())
        {
            qFatal("Could not resolve host: %s: %s", argv[1],
                   host_info.errorString().toUtf8().constData());
            exit(1);
        }
        dest_addr = host_info.addresses().first();
        // Increase the FFT send interval when sending data over the network
        fft_send_interval *= 4;
    } else {
        dest_addr = QHostAddress::LocalHost;
    }

    qDebug() << "Sending FFT data to" << dest_addr << "port" << TRANSMIT_PORT
             << "interval" << fft_send_interval;

    JackClient jc(fft_send_interval);
    Networking net(dest_addr, TRANSMIT_PORT);

    QObject::connect(&jc, SIGNAL(onset_detected()), &net, SLOT(transmit_onset()));
    QObject::connect(&jc, SIGNAL(fft_data(int, float*)), &net, SLOT(transmit_fft_data(int, float*)));
    QObject::connect(&jc, SIGNAL(pitch_data(float,float)), &net, SLOT(transmit_pitch_data(float,float)));

    return app.exec();
}
int main() {
	int a, b;
	scanf_s("%d", &a);
	int jc(int n);
	b = jc(a);
	printf("%d\n", b);

	return 0;

}
int main()
{
	long b,n;
	cin>>b>>n;
	while(b!=0&&n!=0)
	{
		for(long a=0;a<=b;a++)
		{
			if(jc(a,n)>=b)
			{
				if((b-jc(a-1,n))>(jc(a,n)-b))
					cout<<a<<endl;
				else
					cout<<a-1<<endl;
				break;
			}
		}
		cin>>b>>n;
	}
	return 0;
}
Exemple #5
0
int main(int argc, char const* argv[])
{
	iRemoconClient ir("192.168.0.3", 51013);
	Interpreter ip;

	if (!ip.learn_commands_from_xml("data/commands.xml")) {
		std::cerr << "Error: XML is invalid" << std::endl;
	}

	JuliusClient jc("192.168.0.10", 10500);
	jc.connect(ip, ir);

	return 0;
}
Exemple #6
0
    void BMASwapRateHelper::initializeDates() {
        // if the evaluation date is not a business day
        // then move to the next business day
        JointCalendar jc(calendar_,
                         iborIndex_->fixingCalendar());
        Date referenceDate = jc.adjust(evaluationDate_);
        earliestDate_ =
            calendar_.advance(referenceDate, settlementDays_ * Days, Following);

        Date maturity = earliestDate_ + tenor_;

        // dummy BMA index with curve/swap arguments
        shared_ptr<BMAIndex> clonedIndex(new BMAIndex(termStructureHandle_));

        Schedule bmaSchedule =
            MakeSchedule().from(earliestDate_).to(maturity)
                          .withTenor(bmaPeriod_)
                          .withCalendar(bmaIndex_->fixingCalendar())
                          .withConvention(bmaConvention_)
                          .backwards();

        Schedule liborSchedule =
            MakeSchedule().from(earliestDate_).to(maturity)
                          .withTenor(iborIndex_->tenor())
                          .withCalendar(iborIndex_->fixingCalendar())
                          .withConvention(iborIndex_->businessDayConvention())
                          .endOfMonth(iborIndex_->endOfMonth())
                          .backwards();

        swap_ = shared_ptr<BMASwap>(new BMASwap(BMASwap::Payer, 100.0,
                                                liborSchedule,
                                                0.75, // arbitrary
                                                0.0,
                                                iborIndex_,
                                                iborIndex_->dayCounter(),
                                                bmaSchedule,
                                                clonedIndex,
                                                bmaDayCount_));
        swap_->setPricingEngine(shared_ptr<PricingEngine>(new
            DiscountingSwapEngine(iborIndex_->forwardingTermStructure())));

        Date d = calendar_.adjust(swap_->maturityDate(), Following);
        Weekday w = d.weekday();
        Date nextWednesday = (w >= 4) ?
            d + (11 - w) * Days :
            d + (4 - w) * Days;
        latestDate_ = clonedIndex->valueDate(
                         clonedIndex->fixingCalendar().adjust(nextWednesday));
    }
Exemple #7
0
void Quad3DLagrangeP1::jacobian(const MappedCoordsT& mappedCoord, const NodeMatrixT& nodes, JacobianT& result)
{
  JacobianCoefficients jc(nodes);

  const Real xi = mappedCoord[KSI];
  const Real eta = mappedCoord[ETA];

  result(KSI,XX) = jc.bx + jc.dx*eta;
  result(KSI,YY) = jc.by + jc.dy*eta;
  result(KSI,ZZ) = jc.bz + jc.dz*eta;

  result(ETA,XX) = jc.cx + jc.dx*xi;
  result(ETA,YY) = jc.cy + jc.dy*xi;
  result(ETA,ZZ) = jc.cz + jc.dz*xi;
}
Exemple #8
0
void Quad3D::compute_jacobian<Quad3D::JacobianT>(const MappedCoordsT& mapped_coord, const NodesT& nodes, JacobianT& result)
{
  JacobianCoefficients jc(nodes);

  const Real xi = mapped_coord[KSI];
  const Real eta = mapped_coord[ETA];

  result(KSI,XX) = jc.bx + jc.dx*eta;
  result(KSI,YY) = jc.by + jc.dy*eta;
  result(KSI,ZZ) = jc.bz + jc.dz*eta;

  result(ETA,XX) = jc.cx + jc.dx*xi;
  result(ETA,YY) = jc.cy + jc.dy*xi;
  result(ETA,ZZ) = jc.cz + jc.dz*xi;
}
Exemple #9
0
int main()
{
	int e,i;
	double sum=0;
	printf("n e\n");
	printf("- -----------\n");
	for(i=0;i<10;i++)
	{
		sum=(double)sum+1/(double)jc(i);
		if(i<2)
		printf("%d %d\n",i,(int)sum);
		else if(i==2)
		printf("%d %.1f\n",i,sum);
		else
		printf("%d %.9f\n",i,sum);
	}
	system("pause");
}
Exemple #10
0
void nonMaximaSuppression(const Mat& src, const int sz, Mat& dst, const Mat mask) {

    // initialise the block mask and destination
    const int M = src.rows;
    const int N = src.cols;
    const bool masked = !mask.empty();
    Mat block = 255*Mat_<uint8_t>::ones(Size(2*sz+1,2*sz+1));
    dst = Mat_<uint8_t>::zeros(src.size());

    // iterate over image blocks
    for (int m = 0; m < M; m+=sz+1) {
            for (int n = 0; n < N; n+=sz+1) {
                    Point  ijmax;
                    double vcmax, vnmax;

                    // get the maximal candidate within the block
                    Range ic(m, min(m+sz+1,M));
                    Range jc(n, min(n+sz+1,N));
                    minMaxLoc(src(ic,jc), NULL, &vcmax, NULL, &ijmax, masked ? mask(ic,jc) : noArray());
                    Point cc = ijmax + Point(jc.start,ic.start);

                    // search the neighbours centered around the candidate for the true maxima
                    Range in(max(cc.y-sz,0), min(cc.y+sz+1,M));
                    Range jn(max(cc.x-sz,0), min(cc.x+sz+1,N));

                    // mask out the block whose maxima we already know
                    Mat_<uint8_t> blockmask;
                    block(Range(0,in.size()), Range(0,jn.size())).copyTo(blockmask);
                    Range iis(ic.start-in.start, min(ic.start-in.start+sz+1, in.size()));
                    Range jis(jc.start-jn.start, min(jc.start-jn.start+sz+1, jn.size()));
                    blockmask(iis, jis) = Mat_<uint8_t>::zeros(Size(jis.size(),iis.size()));

                    minMaxLoc(src(in,jn), NULL, &vnmax, NULL, &ijmax, masked ? mask(in,jn).mul(blockmask) : blockmask);
                    Point cn = ijmax + Point(jn.start, in.start);

                    // if the block centre is also the neighbour centre, then it's a local maxima
                    if (vcmax > vnmax) {
                            dst.at<uint8_t>(cc.y, cc.x) = src.at<uint8_t>(cc.y, cc.x);
                    }

            }
    }
}
Exemple #11
0
long long ktl(int n) {
    return jc(2 * n) / jc(n + 1) / jc(n);
}
int jc(int n) {
	if (n <= 1)
		return 1;
	else return n*jc(n - 1);

}
TEST_F(LoadPlanningModelsPr2, MixedJointAndIkSamplerManager)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  ks.update();
  robot_state::RobotState ks_const(kmodel);
  ks_const.setToDefaultValues();
  ks_const.update();

  moveit_msgs::Constraints con;
  con.joint_constraints.resize(1);

  con.joint_constraints[0].joint_name = "torso_lift_joint";
  con.joint_constraints[0].position = ks.getVariablePosition("torso_lift_joint");
  con.joint_constraints[0].tolerance_above = 0.01;
  con.joint_constraints[0].tolerance_below = 0.01;
  con.joint_constraints[0].weight = 1.0;

  kinematic_constraints::JointConstraint jc(kmodel);
  EXPECT_TRUE(jc.configure(con.joint_constraints[0]));

  con.position_constraints.resize(1);

  con.position_constraints[0].link_name = "l_wrist_roll_link";
  con.position_constraints[0].target_point_offset.x = 0;
  con.position_constraints[0].target_point_offset.y = 0;
  con.position_constraints[0].target_point_offset.z = 0;
  con.position_constraints[0].constraint_region.primitives.resize(1);
  con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
  con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
  con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;

  con.position_constraints[0].constraint_region.primitive_poses.resize(1);
  con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
  con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
  con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
  con.position_constraints[0].weight = 1.0;

  con.position_constraints[0].header.frame_id = kmodel->getModelFrame();

  con.orientation_constraints.resize(1);
  con.orientation_constraints[0].link_name = "l_wrist_roll_link";
  con.orientation_constraints[0].header.frame_id = kmodel->getModelFrame();
  con.orientation_constraints[0].orientation.x = 0.0;
  con.orientation_constraints[0].orientation.y = 0.0;
  con.orientation_constraints[0].orientation.z = 0.0;
  con.orientation_constraints[0].orientation.w = 1.0;
  con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
  con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
  con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
  con.orientation_constraints[0].weight = 1.0;

  constraint_samplers::ConstraintSamplerPtr s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms_and_torso", con);

  constraint_samplers::UnionConstraintSampler* ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
  ASSERT_TRUE(ucs);

  constraint_samplers::IKConstraintSampler* ikcs_test  = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
  ASSERT_TRUE(ikcs_test);

  for (int t = 0 ; t < 1; ++t)
  {
    EXPECT_TRUE(s->sample(ks, ks_const, 100));
    EXPECT_TRUE(jc.decide(ks).satisfied);
    EXPECT_TRUE(ikcs_test->getPositionConstraint()->decide(ks).satisfied);
    EXPECT_TRUE(ikcs_test->getOrientationConstraint()->decide(ks).satisfied);
  }
}
Exemple #14
0
bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                         const moveit_msgs::MotionPlanRequest& req, const chomp::ChompParameters& params,
                         moveit_msgs::MotionPlanDetailedResponse& res) const
{
  if (!planning_scene)
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "No planning scene initialized.");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
    return false;
  }

  if (req.start_state.joint_state.position.empty())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state is empty");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  if (not planning_scene->getRobotModel()->satisfiesPositionBounds(req.start_state.joint_state.position.data()))
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  ros::WallTime start_time = ros::WallTime::now();
  ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name);

  jointStateToArray(planning_scene->getRobotModel(), req.start_state.joint_state, req.group_name,
                    trajectory.getTrajectoryPoint(0));

  if (req.goal_constraints.empty())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "No goal constraints specified!");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  if (req.goal_constraints[0].joint_constraints.empty())
  {
    ROS_ERROR_STREAM("Only joint-space goals are supported");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  int goal_index = trajectory.getNumPoints() - 1;
  trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
  sensor_msgs::JointState js;
  for (const moveit_msgs::JointConstraint& joint_constraint : req.goal_constraints[0].joint_constraints)
  {
    js.name.push_back(joint_constraint.joint_name);
    js.position.push_back(joint_constraint.position);
    ROS_INFO_STREAM_NAMED("chomp_planner", "Setting joint " << joint_constraint.joint_name << " to position "
                                                            << joint_constraint.position);
  }
  jointStateToArray(planning_scene->getRobotModel(), js, req.group_name, trajectory.getTrajectoryPoint(goal_index));

  const moveit::core::JointModelGroup* model_group =
      planning_scene->getRobotModel()->getJointModelGroup(req.group_name);
  // fix the goal to move the shortest angular distance for wrap-around joints:
  for (size_t i = 0; i < model_group->getActiveJointModels().size(); i++)
  {
    const moveit::core::JointModel* model = model_group->getActiveJointModels()[i];
    const moveit::core::RevoluteJointModel* revolute_joint =
        dynamic_cast<const moveit::core::RevoluteJointModel*>(model);

    if (revolute_joint != nullptr)
    {
      if (revolute_joint->isContinuous())
      {
        double start = (trajectory)(0, i);
        double end = (trajectory)(goal_index, i);
        ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
        (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
      }
    }
  }

  const std::vector<std::string>& active_joint_names = model_group->getActiveJointModelNames();
  const Eigen::MatrixXd goal_state = trajectory.getTrajectoryPoint(goal_index);
  moveit::core::RobotState goal_robot_state = planning_scene->getCurrentState();
  goal_robot_state.setVariablePositions(
      active_joint_names, std::vector<double>(goal_state.data(), goal_state.data() + active_joint_names.size()));

  if (not goal_robot_state.satisfiesBounds())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  // fill in an initial trajectory based on user choice from the chomp_config.yaml file
  if (params.trajectory_initialization_method_.compare("quintic-spline") == 0)
    trajectory.fillInMinJerk();
  else if (params.trajectory_initialization_method_.compare("linear") == 0)
    trajectory.fillInLinearInterpolation();
  else if (params.trajectory_initialization_method_.compare("cubic") == 0)
    trajectory.fillInCubicInterpolation();
  else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0)
  {
    if (!(trajectory.fillInFromTrajectory(res)))
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Input trajectory has less than 2 points, "
                                              "trajectory must contain at least start and goal state");
      return false;
    }
  }
  else
    ROS_ERROR_STREAM_NAMED("chomp_planner", "invalid interpolation method specified in the chomp_planner file");

  ROS_INFO_NAMED("chomp_planner", "CHOMP trajectory initialized using method: %s ",
                 (params.trajectory_initialization_method_).c_str());

  // optimize!
  moveit::core::RobotState start_state(planning_scene->getCurrentState());
  moveit::core::robotStateMsgToRobotState(req.start_state, start_state);
  start_state.update();

  ros::WallTime create_time = ros::WallTime::now();

  int replan_count = 0;
  bool replan_flag = false;
  double org_learning_rate = 0.04, org_ridge_factor = 0.0, org_planning_time_limit = 10;
  int org_max_iterations = 200;

  // storing the initial chomp parameters values
  org_learning_rate = params.learning_rate_;
  org_ridge_factor = params.ridge_factor_;
  org_planning_time_limit = params.planning_time_limit_;
  org_max_iterations = params.max_iterations_;

  std::unique_ptr<ChompOptimizer> optimizer;

  // create a non_const_params variable which stores the non constant version of the const params variable
  ChompParameters params_nonconst = params;

  // while loop for replanning (recovery behaviour) if collision free optimized solution not found
  while (true)
  {
    if (replan_flag)
    {
      // increase learning rate in hope to find a successful path; increase ridge factor to avoid obstacles; add 5
      // additional secs in hope to find a solution; increase maximum iterations
      params_nonconst.setRecoveryParams(params_nonconst.learning_rate_ + 0.02, params_nonconst.ridge_factor_ + 0.002,
                                        params_nonconst.planning_time_limit_ + 5, params_nonconst.max_iterations_ + 50);
    }

    // initialize a ChompOptimizer object to load up the optimizer with default parameters or with updated parameters in
    // case of a recovery behaviour
    optimizer.reset(new ChompOptimizer(&trajectory, planning_scene, req.group_name, &params_nonconst, start_state));
    if (!optimizer->isInitialized())
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Could not initialize optimizer");
      res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
      return false;
    }

    ROS_DEBUG_NAMED("chomp_planner", "Optimization took %f sec to create",
                    (ros::WallTime::now() - create_time).toSec());

    bool optimization_result = optimizer->optimize();

    // replan with updated parameters if no solution is found
    if (params_nonconst.enable_failure_recovery_)
    {
      ROS_INFO_NAMED("chomp_planner", "Planned with Chomp Parameters (learning_rate, ridge_factor, "
                                      "planning_time_limit, max_iterations), attempt: # %d ",
                     (replan_count + 1));
      ROS_INFO_NAMED("chomp_planner", "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ",
                     params_nonconst.learning_rate_, params_nonconst.ridge_factor_,
                     params_nonconst.planning_time_limit_, params_nonconst.max_iterations_);

      if (!optimization_result && replan_count < params_nonconst.max_recovery_attempts_)
      {
        replan_count++;
        replan_flag = true;
      }
      else
      {
        break;
      }
    }
    else
      break;
  }  // end of while loop

  // resetting the CHOMP Parameters to the original values after a successful plan
  params_nonconst.setRecoveryParams(org_learning_rate, org_ridge_factor, org_planning_time_limit, org_max_iterations);

  ROS_DEBUG_NAMED("chomp_planner", "Optimization actually took %f sec to run",
                  (ros::WallTime::now() - create_time).toSec());
  create_time = ros::WallTime::now();
  // assume that the trajectory is now optimized, fill in the output structure:

  ROS_DEBUG_NAMED("chomp_planner", "Output trajectory has %d joints", trajectory.getNumJoints());

  res.trajectory.resize(1);

  res.trajectory[0].joint_trajectory.joint_names = active_joint_names;

  res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header;  // @TODO this is probably a hack

  // fill in the entire trajectory
  res.trajectory[0].joint_trajectory.points.resize(trajectory.getNumPoints());
  for (int i = 0; i < trajectory.getNumPoints(); i++)
  {
    res.trajectory[0].joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
    for (size_t j = 0; j < res.trajectory[0].joint_trajectory.points[i].positions.size(); j++)
    {
      res.trajectory[0].joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
    }
    // Setting invalid timestamps.
    // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints.
    res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
  }

  ROS_DEBUG_NAMED("chomp_planner", "Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec());
  ROS_DEBUG_NAMED("chomp_planner", "Serviced planning request in %f wall-seconds, trajectory duration is %f",
                  (ros::WallTime::now() - start_time).toSec(),
                  res.trajectory[0].joint_trajectory.points[goal_index].time_from_start.toSec());
  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  res.processing_time.push_back((ros::WallTime::now() - start_time).toSec());

  // report planning failure if path has collisions
  if (not optimizer->isCollisionFree())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Motion plan is invalid.");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
    return false;
  }

  // check that final state is within goal tolerances
  kinematic_constraints::JointConstraint jc(planning_scene->getRobotModel());
  robot_state::RobotState last_state(start_state);
  last_state.setVariablePositions(res.trajectory[0].joint_trajectory.joint_names,
                                  res.trajectory[0].joint_trajectory.points.back().positions);

  bool constraints_are_ok = true;
  for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints[0].joint_constraints)
  {
    constraints_are_ok = constraints_are_ok and jc.configure(constraint);
    constraints_are_ok = constraints_are_ok and jc.decide(last_state).satisfied;
    if (not constraints_are_ok)
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal constraints are violated: " << constraint.joint_name);
      res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED;
      return false;
    }
  }

  return true;
}