Eigen::VectorXd L2R_Huber_SVC::productXtVsub2(const Eigen::VectorXd &V) {
Eigen::VectorXd ans = Eigen::VectorXd::Zero(n);
int sizeD = indexs_D2.size();
for (int i = 0; i < sizeD; ++i) {
for (Eigen::SparseMatrix<double, 1, std::ptrdiff_t>::InnerIterator it(
X, indexs_D2[i]);
it; ++it) {
ans.coeffRef(it.index()) += V.coeffRef(i) * it.value();
}
}
return ans;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "parser_test");
ros::NodeHandle nh;
try
{
std::string name = "red_arm";
RobotPtr robot = std::make_shared<Robot>(name);
ParserPtr parser = std::make_shared<Parser>();
std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/red_arm.yaml";
parser->load(path, robot);
ros::MultiThreadedSpinner spinner;
TfPublisherPtr tf_publisher = std::make_shared<TfPublisher>();
const std::string mnp_name = "mnp";
unsigned long cnt = 0;
ros::Rate r(10.0);
while(ros::ok())
{
Eigen::VectorXd q = Eigen::VectorXd::Zero(robot->getDOF(mnp_name));
ManipulatorPtr mnp = robot->getManipulator(mnp_name);
double goal = sin(2.0 * M_PI * 0.2 * cnt * 0.1);
++cnt;
for(uint32_t i = 0; i < q.rows(); ++i)
{
q.coeffRef(6) = M_PI / 4.0 * goal;
}
std::cout << M_PI / 4.0 * goal << std::endl;
//q = Eigen::VectorXd::Constant(q.rows(), 0.0 * M_PI / 4.0);
robot->update(q);
robot->computeJacobian(mnp_name);
robot->computeMassMatrix(mnp_name);
M = robot->getMassMatrix(mnp_name);
J = robot->getJacobian(mnp_name, "gripper");
calc();
tf_publisher->publish(robot, false);
r.sleep();
}
}
catch(ahl_utils::Exception& e)
{
ROS_ERROR_STREAM(e.what());
}
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "parser_test");
ros::NodeHandle nh;
try
{
std::string name = "youbot";
RobotPtr robot = RobotPtr(new Robot(name));
ParserPtr parser = ParserPtr(new Parser());
std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/youbot.yaml";
parser->load(path, robot);
robot->getMobility()->print();
ros::MultiThreadedSpinner spinner;
TfPublisherPtr tf_publisher = TfPublisherPtr(new TfPublisher());
const std::string mnp_name = "mnp";
unsigned long cnt = 0;
const double period = 0.001;
ros::Rate r(1 / period);
ahl_filter::DifferentiatorPtr differentiator = ahl_filter::DifferentiatorPtr(new ahl_filter::PseudoDifferentiator(period, 1.0));
Eigen::VectorXd q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 0.0);
Eigen::VectorXd dq = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 0.0);
differentiator->init(q, dq);
Eigen::VectorXd pre_q = q;
//std::ofstream ofs1;
//ofs1.open("result1.hpp");
//std::ofstream ofs2;
//ofs2.open("result2.hpp");
//std::ofstream ofs3;
//ofs3.open("result3.hpp");
while(ros::ok())
{
q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 1.0);
double coeff = 1.0 * sin(2.0 * M_PI * 0.1 * cnt * period);
++cnt;
q = coeff * q;
//q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), M_PI / 2.0);
q.coeffRef(0) = 0.0;
q.coeffRef(1) = 0.0;
q.coeffRef(2) = 0.0;
robot->update(mnp_name, q);
robot->computeBasicJacobian(mnp_name);
robot->computeMassMatrix(mnp_name);
differentiator->apply(q);
Eigen::VectorXd dq1 = robot->getJointVelocity(mnp_name);
Eigen::VectorXd dq2;
differentiator->copyDerivativeValueTo(dq2);
std::cout << "p : " << q.transpose() << std::endl;
std::cout << "pre_p : " << pre_q.transpose() << std::endl;
Eigen::VectorXd dq3 = (q - pre_q) / period;
pre_q = q;
//std::cout << "dq1 : " << dq1.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
//std::cout << "dq2 : " << dq2.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
//std::cout << "dq3 : " << dq3.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
//std::cout << dq << std::endl << std::endl;
//std::cout << cos(2.0 * M_PI * 0.1 * cnt * 0.1) << std::endl;
tf_publisher->publish(robot, false);
/*
ofs1 << cnt * period << " ";
ofs2 << cnt * period << " ";
ofs3 << cnt * period << " ";
for(unsigned int i = 0; i < dq.rows() - 3; ++i)
{
ofs1 << dq1[i + 3] << " ";
ofs2 << dq2[i + 3] << " ";
ofs3 << dq3[i + 3] << " ";
}
ofs1 << std::endl;
ofs2 << std::endl;
ofs3 << std::endl;
*/
r.sleep();
}
}
catch(ahl_robot::Exception& e)
{
ROS_ERROR_STREAM(e.what());
}
return 0;
}