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, "lwr_server");
ros::NodeHandle nh;
ros::Timer timer_update_model = nh.createTimer(ros::Duration(0.01), updateModel);
ros::Timer timer_control = nh.createTimer(ros::Duration(0.001), control);
robot = RobotPtr(new Robot("lwr"));
ParserPtr parser = ParserPtr(new Parser());
std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/lwr.yaml";
parser->load(path, robot);
controller = RobotControllerPtr(new RobotController());
controller->init(robot);
using namespace ahl_gazebo_if;
gazebo_interface = GazeboInterfacePtr(new GazeboInterface());
gazebo_interface->setDuration(0.010);
gazebo_interface->addJoint("lwr::joint1");
gazebo_interface->addJoint("lwr::joint2");
gazebo_interface->addJoint("lwr::joint3");
gazebo_interface->addJoint("lwr::joint4");
gazebo_interface->addJoint("lwr::joint5");
gazebo_interface->addJoint("lwr::joint6");
gazebo_interface->addJoint("lwr::joint7");
gazebo_interface->connect();
tf_pub = TfPublisherPtr(new TfPublisher());
ManipulatorPtr mnp = robot->getManipulator("mnp");
gravity_compensation = TaskPtr(new GravityCompensation(robot));
damping = TaskPtr(new Damping(mnp));
joint_control = TaskPtr(new JointControl(mnp));
joint_limit = TaskPtr(new JointLimit(mnp, 0.087));
position_control = TaskPtr(new PositionControl(mnp, "gripper", 0.001));
orientation_control = TaskPtr(new OrientationControl(mnp, "gripper", 0.001));
controller->addTask(gravity_compensation, 0);
controller->addTask(damping, 0);
controller->addTask(joint_control, 0);
controller->addTask(joint_limit, 100);
ros::MultiThreadedSpinner spinner;
spinner.spin();
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;
}