2 #include <ros/console.h>
8 int main(
int argc,
char **argv)
36 double SETPOINT_TORQUE = 10;
39 double PUBLISH_FREQUENCY = 200.0;
42 double STEP_START_TIME = 0.2;
45 double STEP_DURATION = 1.0;
60 ros::Rate check_for_messages_rate(1);
63 ros::Rate send_command_rate(PUBLISH_FREQUENCY);
69 ROS_INFO(
"Waiting for state");
71 check_for_messages_rate.sleep();
78 ROS_INFO(
"Waiting for ready to proceed");
80 check_for_messages_rate.sleep();
87 ROS_INFO(
"Trying to move position to zero");
90 send_command_rate.sleep();
94 for(
int i = 0; i < STEP_START_TIME*PUBLISH_FREQUENCY; i++)
99 send_command_rate.sleep();
103 for(
int i = 0; i < STEP_DURATION*PUBLISH_FREQUENCY; i++)
132 send_command_rate.sleep();
void checkForNewMessages()
bool initialStateReceived()
void setPositionDirectly(double _joint_pos)
void setTorqueDirectly(double _torque)
void setVelocityDirectly(double _joint_vel)
void initializeMotor(double _k_p_pos, double _k_d_pos, double _k_p_vel, double _k_i_vel, double _k_p_torque, double _k_i_torque)
double degToRad(const double &_deg)
The degToRad function converts degrees to radians.
int main(int argc, char **argv)