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 = "personal_robot";
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/pr2.yaml";
parser->load(path, robot);
ros::MultiThreadedSpinner spinner;
TfPublisherPtr tf_publisher = TfPublisherPtr(new TfPublisher());
const std::string mnp_name1 = "left_mnp";
const std::string mnp_name2 = "right_mnp";
unsigned long cnt = 0;
ros::Rate r(10.0);
while(ros::ok())
{
Eigen::VectorXd q = Eigen::VectorXd::Zero(robot->getDOF());
ManipulatorPtr left_mnp = robot->getManipulator(mnp_name1);
double goal = sin(2.0 * M_PI * 0.2 * cnt * 0.1);
++cnt;
//std::cout << M_PI / 4.0 * goal << std::endl;
q = Eigen::VectorXd::Constant(q.rows(), 0.0);
q[10] = goal;
robot->update(q);
//robot->update(mnp_name2, q);
robot->computeJacobian(mnp_name1);
robot->computeJacobian(mnp_name2);
robot->computeMassMatrix(mnp_name1);
robot->computeMassMatrix(mnp_name2);
M1 = robot->getMassMatrix(mnp_name1);
M2 = robot->getMassMatrix(mnp_name2);
J1 = robot->getJacobian(mnp_name1, "gripper_l_link");
J2 = robot->getJacobian(mnp_name2, "gripper_r_link");
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;
}
TEST_F(FeatureConstraintsTest, constrainConfiguration)
{
// setting up environment and robot
OpenRAVE::RaveInitialize();
OpenRAVE::EnvironmentBasePtr env = OpenRAVE::RaveCreateEnvironment();
OpenRAVE::RobotBasePtr herb = env->ReadRobotXMLFile("robots/herb2_padded_nosensors.robot.xml");
env->Add(herb);
OpenRAVE::RobotBase::ManipulatorPtr right_arm;
std::vector<OpenRAVE::RobotBase::ManipulatorPtr> herb_manipulators = herb->GetManipulators();
for (unsigned int i=0; i<herb_manipulators.size(); i++)
{
if(herb_manipulators[i] && (herb_manipulators[i]->GetName().compare("right_wam") == 0))
{
right_arm = herb_manipulators[i];
}
}
herb->SetActiveManipulator(right_arm);
herb->SetActiveDOFs(right_arm->GetArmIndices());
// putting the bottle in the robot's right hand
OpenRAVE::KinBodyPtr bottle_body = OpenRAVE::RaveCreateKinBody(env, "");
ASSERT_TRUE(env->ReadKinBodyURI(bottle_body, "objects/household/fuze_bottle.kinbody.xml"));
bottle_body->SetName("bottle");
env->Add(bottle_body);
OpenRAVE::geometry::RaveTransformMatrix<OpenRAVE::dReal> m;
m.rotfrommat(-0.86354026, 0.50427591, 0.00200643,
-0.25814712, -0.44547139, 0.85727201,
0.43319543, 0.73977094, 0.51485985);
m.trans.x = 1.10322189;
m.trans.y = -0.24693695;
m.trans.z = 0.87205762;
bottle_body->SetTransform(OpenRAVE::Transform(m));
OpenRAVE::Transform bottle_transform1 = bottle_body->GetTransform();
ASSERT_TRUE(herb->Grab(bottle_body));
// adding a flying pan
OpenRAVE::KinBodyPtr pan_body = OpenRAVE::RaveCreateKinBody(env, "");
ASSERT_TRUE(env->ReadKinBodyURI(pan_body, "objects/household/pan.kinbody.xml"));
pan_body->SetName("pan");
env->Add(pan_body);
m.rotfrommat(2.89632833e-03, 9.99995806e-01, 1.19208782e-07,
-9.99995806e-01, 2.89632833e-03, -1.18864027e-07,
-1.19208797e-07, -1.18864013e-07, 1.00000000e+00);
m.trans.x = 0.58;
m.trans.y = -0.02;
m.trans.z = 0.33;
pan_body->SetTransform(OpenRAVE::Transform(m));
// giving constraints access to environment and robot
RobotPtr robot = RobotPtr(new Robot(herb));
EnvironmentPtr environment = EnvironmentPtr(new Environment(env));
constraints.setRobot(robot);
constraints.setEnvironment(environment);
// first test
constraints.calculateConstraintValues();
EXPECT_FALSE(constraints.areConstraintsFulfilled());
// moving robot and bottle to new start configuration
std::vector<double> new_config;
new_config.push_back(2.0);
new_config.push_back(1.0);
new_config.push_back(-0.8);
new_config.push_back(1.0);
new_config.push_back(-1.0);
new_config.push_back(0.0);
new_config.push_back(0.0);
robot->setJointValues(new_config);
constraints.setJointValues(robot->getJointValues());
constraints.calculateConstraintValues();
// test we've really moved
OpenRAVE::Transform bottle_transform2 = bottle_body->GetTransform();
ASSERT_GT((bottle_transform1.trans - bottle_transform2.trans).lengthsqr3(), 0.1);
ASSERT_GT((bottle_transform1.rot - bottle_transform2.rot).lengthsqr4(), 0.1);
// actual projection test
ASSERT_EQ(robot->getDOF(), constraints.getJointValues().size());
std::cout << "\nprior to constraining:\n";
std::vector<double> tmp = constraints.getTaskValues();
for(unsigned int i=0; i<tmp.size(); i++)
{
std::cout << tmp[i] << " ";
}
constraints.constrainCurrentConfiguration();
std::cout << "\npost to constraining:\n";
tmp = constraints.getTaskValues();
for(unsigned int i=0; i<tmp.size(); i++)
{
std::cout << tmp[i] << " ";
}
std::cout << "\n";
EXPECT_TRUE(constraints.areConstraintsFulfilled());
OpenRAVE::RaveDestroy();
}
void control(const ros::TimerEvent&)
{
try
{
boost::mutex::scoped_lock lock(mutex);
if(gazebo_interface->subscribed())
{
Eigen::VectorXd q = gazebo_interface->getJointStates();
robot->update("mnp", q);
joint_updated = true;
}
if(updated)
{
static long cnt = 0;
if(initialized == false)
{
Eigen::VectorXd qd = Eigen::VectorXd::Constant(robot->getDOF("mnp"), M_PI / 4.0);
double sin_val = 1.0;//std::abs(sin(2.0 * M_PI * 0.1 * cnt * 0.001));
qd = sin_val * qd;
joint_control->setGoal(qd);
static int reached = 0;
if(robot->reached("mnp", qd, 0.03))
{
++reached;
if(reached > 500)
{
initialized = true;
controller->clearTask();
controller->addTask(damping, 0);
controller->addTask(gravity_compensation, 100);
controller->addTask(position_control, 10);
controller->addTask(orientation_control, 5);
controller->addTask(joint_limit, 100);
Eigen::Vector3d xd;
xd << 0.35, 0.4, 0.75;
position_control->setGoal(xd);
Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
double rad = 0.0;
R << cos(rad), 0, sin(rad),
0, 1, 0,
-sin(rad), 0, cos(rad);
orientation_control->setGoal(R);
}
}
else
{
reached = 0;
}
}
else
{
Eigen::Vector3d xd;
xd << 0.35, 0.4, 0.75;
//xd.coeffRef(0) += 0.1 * sin(2.0 * M_PI * 0.2 * cnt * 0.001);
//xd.coeffRef(1) += 0.1 * sin(2.0 * M_PI * 0.2 * cnt * 0.001);
//xd.coeffRef(2) += 0.1 * sin(2.0 * M_PI * 0.2 * cnt * 0.001);
position_control->setGoal(xd);
Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
double rad = M_PI / 3.0 * sin(2.0 * M_PI * 0.2 * cnt * 0.001);
R << cos(rad), 0, sin(rad),
0, 1, 0,
-sin(rad), 0, cos(rad);
orientation_control->setGoal(R);
}
++cnt;
Eigen::VectorXd tau(robot->getDOF("mnp"));
controller->computeGeneralizedForce(tau);
gazebo_interface->applyJointEfforts(tau);
}
}
catch(ahl_ctrl::Exception& e)
{
ROS_ERROR_STREAM(e.what());
}
catch(ahl_gazebo_if::Exception& e)
{
ROS_ERROR_STREAM(e.what());
}
}
