arm draw a circle problem
my node programs:
#ifdef __i386__
#pragma message("i386 Architecture detected, disabling EIGEN VECTORIZATION")
#define EIGEN_DONT_VECTORIZE
#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
#else
#pragma message("64bit Architecture detected, enabling EIGEN VECTORIZATION")
#endif
#include<ros/ros.h>
#include<moveit/move_group_interface/move_group.h>
#include <descartes_moveit/moveit_state_adapter.h>
#include<eigen3/Eigen/Geometry>
#include<eigen_conversions/eigen_msg.h>
int main(int argc, char *argv[])
{
ros::init(argc,argv,"lesson_move_group_node");
//start a background "spinner",so the node can process ROS messages
ros::AsyncSpinner spinner(1);
spinner.start();
//create a Descartes robot model
descartes_core::RobotModelPtr robot_model_ptr(new descartes_moveit::MoveitStateAdapter());
if(robot_model_ptr->initialize("robot_description","manipulator","base","tool0"))
{
ROS_INFO_STREAM("Robot Model initialized");
}
else
{
ROS_ERROR_STREAM("Failed to initialized Robot Model");
return 1;
}
moveit::planning_interface::MoveGroup group("manipulator");
std::vector<double> joints(robot_model_ptr->getDOF(),0);
//std::vector< std::vector<double> > solutions;
std::vector<double>solution;
Eigen::Affine3d tool_pose;
const double radius(0.10);
double X(0.0),Y(0.0),angle(0.0);
while(ros::ok())
{
angle=angle*3.141592/180.0;
X=radius*sin(angle);
Y=radius*cos(angle);
tool_pose=Eigen::Translation3d(X, Y, 0.390)
* Eigen::Quaterniond(-0.014, 0.733, 0.680, -0.010);
if(robot_model_ptr->getIK(tool_pose,joints,solution))
{
ROS_INFO_STREAM("Found IK solution: ["<<solution[0]<<", "
<<solution[1]<<", "
<<solution[2]<<", "
<<solution[3]<<", "
<<solution[4]<<", "
<<solution[5]<<"] ");
}
else
{
ROS_ERROR_STREAM("IK failed");
return 1;
}
for(unsigned int i=0;i<6;i++)
{
joints[i]=solution[i];
}
angle=(angle+10.0)/360.0;
group.setJointValueTarget(joints);
group.move();
//sleep(0.3);
}
}
it just move once,and never move again?I followed the tutorials ROS-Industrial exercises 3.6 and exercises 4.1! I don't know why?
Asked by WuJeff on 2016-05-14 09:15:17 UTC
Comments