int GraspitDynamics::stepDynamics() { double actualTimeStep = moveDynamicBodies(mWorld->getTimeStep()); if (actualTimeStep < 0) { GraspitDynamics::turnOffDynamics(); mWorld->emitdynamicsError("Timestep failsafe reached."); return -1; } for (int i = 0; i < mWorld->getNumRobots(); i++) { if (runController) { mWorld->getRobot(i)->DOFController(actualTimeStep); } mWorld->getRobot(i)->applyJointPassiveInternalWrenches(); } if (computeNewVelocities(actualTimeStep)) { mWorld->emitdynamicsError("LCP could not be solved."); return -1; } mWorld->resetDynamicWrenches(); return 0; }
/*! This is the main routine for parsing input on the clientSocket. There should be one command for each line of input. This reads one line, and looks at the first word (up to the first space character) to determine the command. Then if there are body or robot indices to read, it calls a support routine to read those and return a vector of bodies or robots. These are then passed to the appropriate routine to carry out the action and write out any necessary results. */ void ClientSocket::readClient() { int i, numData, numBodies, numRobots; double time; std::vector<Body *> bodyVec; std::vector<Robot *> robVec; bool ok; while (canReadLine()) { line = readLine(); line.truncate(line.length() - 1); //strip newline character lineStrList = QStringList::split(' ', line); strPtr = lineStrList.begin(); #ifdef GRASPITDBG std::cout << "Command parser line: " << line.toStdString().c_str() << std::endl; #endif if (*strPtr == "getContacts") { strPtr++; if (strPtr == lineStrList.end()) { continue; } numData = (*strPtr).toInt(&ok); strPtr++; if (!ok) { continue; } #ifdef GRASPITDBG std::cout << "Num data: " << numData << std::endl; #endif if (readBodyIndList(bodyVec)) { continue; } numBodies = bodyVec.size(); for (i = 0; i < numBodies; i++) { sendContacts(bodyVec[i], numData); } } else if (*strPtr == "getAverageContacts") { strPtr++; if (readBodyIndList(bodyVec)) { continue; } numBodies = bodyVec.size(); for (i = 0; i < numBodies; i++) { sendAverageContacts(bodyVec[i]); } } else if (*strPtr == "getBodyName") { strPtr++; if (readBodyIndList(bodyVec)) { continue; } numBodies = bodyVec.size(); for (i = 0; i < numBodies; i++) { sendBodyName(bodyVec[i]); } } else if (*strPtr == "getRobotName") { strPtr++; if (readRobotIndList(robVec)) { continue; } numRobots = robVec.size(); for (i = 0; i < numRobots; i++) { sendRobotName(robVec[i]); } } else if (*strPtr == "getDOFVals") { strPtr++; if (readRobotIndList(robVec)) { continue; } numRobots = robVec.size(); for (i = 0; i < numRobots; i++) { sendDOFVals(robVec[i]); } } else if (*strPtr == "moveDOFs") { strPtr++; readDOFVals(); } else if (*strPtr == "render") { graspitCore->getIVmgr()->getViewer()->render(); } else if (*strPtr == "setDOFForces") { strPtr++; if (readRobotIndList(robVec)) { continue; } numRobots = robVec.size(); for (i = 0; i < numRobots; i++) if (readDOFForces(robVec[i]) == FAILURE) { continue; } } else if (*strPtr == "moveToContacts") { graspitCore->getWorld()->getCurrentHand()->approachToContact(30, true); } else if ((*strPtr) == "moveDynamicBodies") { strPtr++; if (strPtr == lineStrList.end()) { ok = FALSE; } else { time = (*strPtr).toDouble(&ok); strPtr++; } if (!ok) { moveDynamicBodies(-1); } else { moveDynamicBodies(time); } } else if (*strPtr == "computeNewVelocities") { #ifdef GRASPITDBG std::cout << "cnv" << std::endl; #endif strPtr++; if (strPtr == lineStrList.end()) { continue; } time = (*strPtr).toDouble(&ok); strPtr++; if (!ok) { continue; } #ifdef GRASPITDBG std::cout << time << std::endl; #endif computeNewVelocities(time); } } }
/** * @param pos current position of robot * @param vel desired velocity for sampling */ bool SimpleTrajectoryGenerator::generateTrajectory( Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f sample_target_vel, base_local_planner::Trajectory& traj) { double vmag = ::hypot(sample_target_vel[0], sample_target_vel[1]); double eps = 1e-4; traj.cost_ = -1.0; // placed here in case we return early //trajectory might be reused so we'll make sure to reset it traj.resetPoints(); // make sure that the robot would at least be moving with one of // the required minimum velocities for translation and rotation (if set) if ((limits_->min_trans_vel >= 0 && vmag + eps < limits_->min_trans_vel) && (limits_->min_rot_vel >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_rot_vel)) { return false; } // make sure we do not exceed max diagonal (x+y) translational velocity (if set) if (limits_->max_trans_vel >=0 && vmag - eps > limits_->max_trans_vel) { return false; } int num_steps; if (discretize_by_time_) { num_steps = ceil(sim_time_ / sim_granularity_); } else { //compute the number of steps we must take along this trajectory to be "safe" double sim_time_distance = vmag * sim_time_; // the distance the robot would travel in sim_time if it did not change velocity double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time num_steps = ceil(std::max(sim_time_distance / sim_granularity_, sim_time_angle / angular_sim_granularity_)); } //compute a timestep double dt = sim_time_ / num_steps; traj.time_delta_ = dt; Eigen::Vector3f loop_vel; if (continued_acceleration_) { // assuming the velocity of the first cycle is the one we want to store in the trajectory object loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt); traj.xv_ = loop_vel[0]; traj.yv_ = loop_vel[1]; traj.thetav_ = loop_vel[2]; } else { // assuming sample_vel is our target velocity within acc limits for one timestep loop_vel = sample_target_vel; traj.xv_ = sample_target_vel[0]; traj.yv_ = sample_target_vel[1]; traj.thetav_ = sample_target_vel[2]; } //simulate the trajectory and check for collisions, updating costs along the way for (int i = 0; i < num_steps; ++i) { //add the point to the trajectory so we can draw it later if we want traj.addPoint(pos[0], pos[1], pos[2]); if (continued_acceleration_) { //calculate velocities loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt); //ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]); } //update the position of the robot using the velocities passed in pos = computeNewPositions(pos, loop_vel, dt); } // end for simulation steps return num_steps > 0; // true if trajectory has at least one point }
/*! This is the main routine for parsing input on the clientSocket. There should be one command for each line of input. This reads one line, and looks at the first word (up to the first space character) to determine the command. Then if there are body or robot indices to read, it calls a support routine to read those and return a vector of bodies or robots. These are then passed to the appropriate routine to carry out the action and write out any necessary results. */ void ClientSocket::readClient() { int i,numData,numBodies,numRobots; double time; std::vector<Body *> bodyVec; std::vector<Robot *> robVec; bool ok; while ( canReadLine() ) { line = readLine(); line.truncate(line.length()-1); //strip newline character lineStrList = QStringList::split(' ',line); strPtr = lineStrList.begin(); #ifdef GRASPITDBG std::cout <<"Command parser line: "<<line << std::endl; #endif if (*strPtr == "getContacts") { strPtr++; if (strPtr == lineStrList.end()) continue; numData = (*strPtr).toInt(&ok); strPtr++; if (!ok) continue; #ifdef GRASPITDBG std::cout << "Num data: "<<numData<<std::endl; #endif if (readBodyIndList(bodyVec)) continue; numBodies = bodyVec.size(); for (i=0;i<numBodies;i++) sendContacts(bodyVec[i],numData); } else if (*strPtr == "getAverageContacts") { strPtr++; if (readBodyIndList(bodyVec)) continue; numBodies = bodyVec.size(); for (i=0;i<numBodies;i++) sendAverageContacts(bodyVec[i]); } else if (*strPtr == "getBodyName") { strPtr++; if (readBodyIndList(bodyVec)) continue; numBodies = bodyVec.size(); for (i=0;i<numBodies;i++) sendBodyName(bodyVec[i]); } else if(*strPtr == "setBodyName") { strPtr++; int body_index; if(strPtr != lineStrList.end()){ body_index = strPtr->toInt(&ok); strPtr++; if(strPtr == lineStrList.end()) return; if (body_index == -1 || body_index >= graspItGUI->getIVmgr()->getWorld()->getNumBodies()) { body_index = graspItGUI->getIVmgr()->getWorld()->getNumBodies() - 1; } graspItGUI->getIVmgr()->getWorld()->getBody(body_index)->setName(*strPtr); } } else if (*strPtr == "getRobotName") { strPtr++; if (readRobotIndList(robVec)) continue; numRobots = robVec.size(); for (i=0;i<numRobots;i++) sendRobotName(robVec[i]); } else if (*strPtr == "getDOFVals") { strPtr++; if (readRobotIndList(robVec)) continue; numRobots = robVec.size(); for (i=0;i<numRobots;i++) sendDOFVals(robVec[i]); } else if (*strPtr == "moveDOFs") { strPtr++; readDOFVals(); } else if (*strPtr == "render") graspItGUI->getIVmgr()->getViewer()->render(); else if (*strPtr == "setDOFForces") { strPtr++; if (readRobotIndList(robVec)) continue; numRobots = robVec.size(); for (i=0;i<numRobots;i++) if (readDOFForces(robVec[i])==FAILURE) continue; } else if ((*strPtr) == "moveDynamicBodies") { strPtr++; if (strPtr == lineStrList.end()) ok = FALSE; else { time = (*strPtr).toDouble(&ok); strPtr++; } if (!ok) moveDynamicBodies(-1); else moveDynamicBodies(time); } else if (*strPtr == "computeNewVelocities") { #ifdef GRASPITDBG std::cout << "cnv" << std::endl; #endif strPtr++; if (strPtr == lineStrList.end()) continue; time = (*strPtr).toDouble(&ok); strPtr++; if (!ok) continue; #ifdef GRASPITDBG std::cout << time <<std::endl; #endif computeNewVelocities(time); } else if ((*strPtr) == "outputPlannerResults"){ strPtr++; outputPlannerResults(0); } else if ((*strPtr) == "outputCurrentGrasp"){ strPtr++; outputCurrentGrasp(); } else if ((*strPtr) == "sendBodyTransf"){ strPtr++; verifyInput(1); sendBodyTransf(); } else if ((*strPtr) == "setBodyTransf"){ strPtr++; verifyInput(7); setBodyTransf(); } else if ((*strPtr) == "addObstacle"){ strPtr++; verifyInput(1); addObstacle(*(strPtr+1)); strPtr+=2; } else if ((*strPtr) == "addObject"){ verifyInput(2); addGraspableBody(*(strPtr+1), *(strPtr+2)); strPtr+=3; verifyInput(7); transf object_pose; readTransf(&object_pose); World * w = graspItGUI->getIVmgr()->getWorld(); w->getGB(w->getNumGB() - 1)->setTran(object_pose); } else if ((*strPtr) == "getCurrentHandTran"){ strPtr++; getCurrentHandTran(); } else if ((*strPtr) == "signalGraspUnreachable"){ strPtr+=4; std::cout << line.toStdString() << std::endl; graspItGUI->getIVmgr()->blinkBackground(); } else if ((*strPtr) == "getPlannerTarget"){ strPtr+=1; QTextStream os (this) ; os << graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->getGrasp()->getObject()->getName() << "\n"; } else if ((*strPtr) == "setPlannerTarget"){ QTextStream os(this); os << setPlannerTarget(*(strPtr+1)) << "\n"; strPtr+=2; } else if ((*strPtr) == "rotateHandLat"){ strPtr+=1; rotateHandLat(); } else if ((*strPtr) == "rotateHandLong"){ strPtr+=1; rotateHandLong(); } else if ((*strPtr) == "exec"){ strPtr+=1; exec(); } else if ((*strPtr) == "next"){ strPtr+=1; next(); } else if ((*strPtr) == "addPointCloud") { strPtr += 1; addPointCloud(); //QTextStream os(this); //os << addPointCloud() <<" \n"; } else if ((*strPtr) == "setCameraOrigin") { strPtr += 1; setCameraOrigin(); } else if ((*strPtr) == "removeBodies"){ strPtr += 1; removeBodies(); } else if ((*strPtr) == "clearGraspableBodies"){ strPtr += 1; removeBodies(true); } } }