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;
}
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;
}
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));
}
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;
}
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;
}
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");
}
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);
}
}
}
}
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);
}
}
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, ¶ms_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;
}
