/** * @function Load * @brief */ void drchubo_Walk1::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) { printf("Calling Load Function \n"); this->mModel = _parent; this->mWorld = this->mModel->GetWorld(); // Fill the actuated joints mActuatedJoints.resize( mNumActuatedJoints ); for( int i = 0; i < mNumActuatedJoints; ++i ) { mActuatedJoints[i] = mModel->GetJoint( mActuatedJointNames[i].c_str() ); } // Read Trajectory file ReadTrajectoryFile(); // Set initial hard-coded pose SetHardCodedInitialPose(); // Initialize control values SetInitControl(); // Save last update time mLastUpdatedTime = mModel->GetWorld()->GetSimTime(); // Set to update every world cycle. Listen to update event this->mUpdateConnection = event::Events::ConnectWorldUpdateBegin( boost::bind(&drchubo_Walk1::UpdateStates, this)); }
int main( int argc, char** argv ) { /*********** Parse the Trajectory file ************/ // parse the trajectory file FILE* fCfg = fopen(argv[1], "r"); if(fCfg == NULL) { printf("ERROR: unable to open %s\n", argv[1]); exit(1); } std::vector<std::vector<float> > traj; traj = ReadTrajectoryFile(fCfg); printf("There are %i steps to this trajectory\n",traj.size()); /*********** Initialize ROS ****************/ ros::init(argc,argv); ros::node *node = new ros::node("test_arm_traj"); set_arm_traj sat; node->subscribe("mechanism_state",sat.in, &set_arm_traj::doNothing,10); signal(SIGINT, finalize); signal(SIGQUIT, finalize); signal(SIGTERM, finalize); /*********** Start moving the robot ************/ double run_time = 25.0; ros::Time iteration_time; bool run_time_set = true; int m,n,g; node->advertise<pr2_mechanism_controllers::JointPosCmd>("left_arm_commands",10); sleep(1); node->publish("left_arm_commands", sat.LeftArmCmd); sleep(1); ros::Time start_time = ros::Time::now(); ros::Duration sleep_time(0.01); //subtract one for now because the goal is all zeros for (m = 0; m < traj.size(); m++) { iteration_time = ros::Time::now(); done = 0; for(n = 0; n < NUMOFLINKS; n++) { sat.LeftArmCmd.positions[n] = double(traj[m].at(n)); } // publish output node->publish("left_arm_commands", sat.LeftArmCmd); while(!done) { // ros::Duration delta_time = ros::Time::now() - start_time; ros::Duration delta_time = ros::Time::now() - iteration_time; if(run_time_set && delta_time.toSec() > run_time) { printf("It's taking too long to get to waypoint #%i...\n\n",m+1); g = 6; for (int i = GRIPPER_ROLL_LEFT_JOINT; i <= SHOULDER_PAN_LEFT_JOINT; i++) { printf("%s: %2.3f (%2.3f)\n", sat.in.joint_states[i].name.c_str(),sat.in.joint_states[i].position, traj[m].at(g)); g--; } printf("\n"); break; } if ((sat.in.joint_states[GRIPPER_ROLL_LEFT_JOINT].position + MOE >= traj[m].at(6) && sat.in.joint_states[GRIPPER_ROLL_LEFT_JOINT].position - MOE <= traj[m].at(6)) && (sat.in.joint_states[WRIST_FLEX_LEFT_JOINT].position + MOE >= traj[m].at(5) && sat.in.joint_states[WRIST_FLEX_LEFT_JOINT].position - MOE <= traj[m].at(5)) && (sat.in.joint_states[FOREARM_ROLL_LEFT_JOINT].position + MOE >= traj[m].at(4) && sat.in.joint_states[FOREARM_ROLL_LEFT_JOINT].position - MOE <= traj[m].at(4)) && (sat.in.joint_states[ELBOW_FLEX_LEFT_JOINT].position + MOE >= traj[m].at(3) && sat.in.joint_states[ELBOW_FLEX_LEFT_JOINT].position - MOE <= traj[m].at(3)) && (sat.in.joint_states[UPPERARM_ROLL_LEFT_JOINT].position + MOE >= traj[m].at(2) && sat.in.joint_states[UPPERARM_ROLL_LEFT_JOINT].position - MOE <= traj[m].at(2)) && (sat.in.joint_states[SHOULDER_PITCH_LEFT_JOINT].position + MOE >= traj[m].at(1) && sat.in.joint_states[SHOULDER_PITCH_LEFT_JOINT].position - MOE <= traj[m].at(1)) && (sat.in.joint_states[SHOULDER_PAN_LEFT_JOINT].position + MOE >= traj[m].at(0) && sat.in.joint_states[SHOULDER_PAN_LEFT_JOINT].position - MOE <= traj[m].at(0))) { printf("\nArm is at waypoint #%i of %i\n\n", m+1, traj.size()); g = 6; for (int i = GRIPPER_ROLL_LEFT_JOINT; i <= SHOULDER_PAN_LEFT_JOINT; i++) { printf("%s: %2.3f (%2.3f)\n", sat.in.joint_states[i].name.c_str(),sat.in.joint_states[i].position, traj[m].at(g)); g--; } printf("\n"); done = 1; } sleep_time.sleep(); } } node->unsubscribe("mechanism_state"); node->unadvertise("left_arm_commands"); ros::fini(); return 0; }