本文整理汇总了C++中TaskPtr::setGoal方法的典型用法代码示例。如果您正苦于以下问题：C++ TaskPtr::setGoal方法的具体用法？C++ TaskPtr::setGoal怎么用？C++ TaskPtr::setGoal使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类TaskPtr的用法示例。
在下文中一共展示了TaskPtr::setGoal方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: control
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());
}
}