void controller_callback(Moby::ControlledBodyPtr dbp, double t, void*)
{
double p = 0.001*cos(t*(2.0*M_PI)*500.0);
double v = -0.001*sin(t*1000.0) *(2.0*M_PI)*500.0;
double Kp = 1e3
,Kv = 1e1
;
for(int i=0;i<slide->get_joints().size();i++){
if(slide->get_joints()[i]->num_dof() == 0) continue;
static Ravelin::VectorNd U(1);
U[0] = Kp*(p - slide->get_joints()[i]->q[0]) + Kv*(v - slide->get_joints()[i]->qd[0]);
std::cout << "x = [ " << slide->get_joints()[i]->q[0] << " , " << slide->get_joints()[i]->qd[0] << " ];" << std::endl;
std::cout << "x* = [ " << p << " , " << v << " ];" << std::endl;
std::cout << "U = [ " << U[0] << " ];" << std::endl;
slide->get_joints()[i]->add_force(U);
}
}
void init(void* separator, const std::map<std::string, Moby::BasePtr>& read_map, double time)
{
const unsigned Z = 2;
// get a reference to the TimeSteppingSimulator instance
for (std::map<std::string, Moby::BasePtr>::const_iterator i = read_map.begin();
i !=read_map.end(); i++)
{
// Find the simulator reference
if (!sim)
sim = boost::dynamic_pointer_cast<TimeSteppingSimulator>(i->second);
if (i->first == "ur10_schunk_hybrid")
robot = boost::dynamic_pointer_cast<RCArticulatedBody>(i->second);
}
assert(robot);
robot->controller = &controller;
// sets the starting velocity for the robot joints (do not modify this)
const std::vector<shared_ptr<Jointd> >& joints = robot->get_joints();
robot->get_generalized_coordinates_euler(tempQ);
for (unsigned i=0; i< joints.size(); i++)
if (joints[i]->joint_id.find("fixed") == std::string::npos &&
joints[i]->joint_id != "world_joint")
q_init[joints[i]->joint_id] = tempQ[joints[i]->get_coord_index()];
// setup the constraint callback function
//sim->constraint_callback_fn = &constraint_callback_fn;
const double PERIOD = 5.0;
const double AMP = 0.5;
const double SMALL_AMP = AMP * 0.1;
std::map<std::string, double> qd_init;
qd_init["shoulder_pan_joint"] = AMP*PERIOD;
qd_init["shoulder_lift_joint"] = SMALL_AMP*PERIOD*2.0;
qd_init["elbow_joint"] = AMP*PERIOD*2.0/3.0;
qd_init["wrist_1_joint"] = AMP*PERIOD*1.0/7.0;
qd_init["wrist_2_joint"] = AMP*PERIOD*2.0/11.0;
qd_init["wrist_3_joint"] = AMP*PERIOD*3.0/13.0;
qd_init["l_finger_actuator"] = 0.0;
qd_init["r_finger_actuator"] = 0.0;
VectorNd qd;
robot->get_generalized_velocity(DynamicBodyd::eEuler, qd);
for (unsigned i=0; i< joints.size(); i++)
qd[joints[i]->get_coord_index()] = qd_init[joints[i]->joint_id];
robot->set_generalized_velocity(DynamicBodyd::eEuler, qd);
// read gains from the gains file
std::ifstream in("gains.dat");
if (in.fail())
{
std::cerr << "Failed to open 'gains.dat' for reading!" << std::endl;
exit(-1);
}
char c;bool stringbool=true; bool P=true; bool I=false; bool D = false;
bool done = false;
std::string gainsString;
std::string jointString;
std::string Num;
std::string pNum;
std::string iNum;
std::string dNum;
int i = 0;
std::stringstream ss;
while(in.get(c)){
ss << c;
}
gainsString = ss.str();
// overwrite any output files
for (std::map<std::string, double>::const_iterator i = q_init.begin(); i != q_init.end(); i++)
{
const std::string& joint_name = i->first;
std::string fname1 = joint_name + "_desiredPID.txt";
std::string fname2 = joint_name + "_statePID.txt";
std::ofstream out1(fname1.c_str());
std::ofstream out2(fname2.c_str());
out1.close();
out2.close();
}
std::string lineString;
// read the step size
char* stepsize_str = getenv("STEP_SIZE");
//std::cout << stepsize_str<<std::endl;
//float stepsize_str = getenv("STEP_SIZE");
if (!stepsize_str)
throw std::runtime_error("No STEP_SIZE variable specified!");
step_size = std::atof(stepsize_str);
//std::cout << step_size << std::endl;
for (int i = 0; i < gainsString.length(); i++){
char c = gainsString[i];//this char is the one that is iterated through in the text file
if ( (c >64 && c < 91) || (c > 96 && c < 123) || c == 95 || (c>47 && c<58)){//corresponds to the available
jointString += c; //ASCII values for letters and numbers, and hyphens
}else if (c == 9){//if a tab is found, means that a number is coming
while (true){i++; c=gainsString[i]; if (c!=9){break;}}//skip all tabs until a number is found
while (true){
if (c == 9){//if a another tab is found that means its the next number
while (true){i++; c=gainsString[i]; if (c!=9){break;}}//skip thru tabs
if (P){P = false; I = true;}//if its time for p gain, we know i gain is next so set bool
else if (I){I = false; D = true;}
else if (D){
D = false; P = true;
}
}else if (c == '\n'){
double pDouble= atof(pNum.c_str());//if a new line is found we know we have obtained all values
double iDouble= atof(iNum.c_str());//so turn the strings into doubles
double dDouble= atof(dNum.c_str());
Ravelin::Origin3d gainValues(pDouble,iDouble,dDouble);//created an O3D of gains
PID_gains.insert(std::pair<std::string,Origin3d>(jointString,gainValues));//insert into map
jointString = "";//reset all found values to begin finding the next row
pNum = "";
iNum = "";
dNum = "";
D = false; P = true;
break;
}
if (P){
pNum += c;//if its time for a p gain to be entered, add the sucker in
}else if (D){
dNum += c;
}else if (I){
iNum += c;
}
i++;
c = gainsString[i];
}
}
}
}
