本文整理汇总了C++中robot_state::RobotState::getVariableAccelerations方法的典型用法代码示例。如果您正苦于以下问题：C++ RobotState::getVariableAccelerations方法的具体用法？C++ RobotState::getVariableAccelerations怎么用？C++ RobotState::getVariableAccelerations使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类robot_state::RobotState的用法示例。
在下文中一共展示了RobotState::getVariableAccelerations方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: doPlan
void doPlan(const std::string& group_name,
planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res,
robot_state::RobotState& start_state,
robot_state::RobotState& goal_state,
planning_scene::PlanningScenePtr& planning_scene,
planning_interface::PlannerManagerPtr& planner_instance)
{
const robot_state::JointModelGroup* joint_model_group =
goal_state.getJointModelGroup("whole_body");
// Copy from start_state to req.start_state
unsigned int num_joints = start_state.getVariableCount();
req.start_state.joint_state.name = start_state.getVariableNames();
req.start_state.joint_state.position.resize(num_joints);
req.start_state.joint_state.velocity.resize(num_joints);
req.start_state.joint_state.effort.resize(num_joints);
memcpy(&req.start_state.joint_state.position[0],
start_state.getVariablePositions(), sizeof(double) * num_joints);
if (start_state.hasVelocities())
memcpy(&req.start_state.joint_state.velocity[0],
start_state.getVariableVelocities(),
sizeof(double) * num_joints);
else
memset(&req.start_state.joint_state.velocity[0], 0,
sizeof(double) * num_joints);
if (start_state.hasAccelerations())
memcpy(&req.start_state.joint_state.effort[0],
start_state.getVariableAccelerations(),
sizeof(double) * num_joints);
else
memset(&req.start_state.joint_state.effort[0], 0,
sizeof(double) * num_joints);
req.group_name = group_name;
moveit_msgs::Constraints joint_goal =
kinematic_constraints::constructGoalConstraints(goal_state,
joint_model_group);
req.goal_constraints.push_back(joint_goal);
// We now construct a planning context that encapsulate the scene,
// the request and the response. We call the planner using this
// planning context
planning_interface::PlanningContextPtr context =
planner_instance->getPlanningContext(planning_scene, req,
res.error_code_);
context->solve(res);
if (res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
exit(0);
}
}