void CMtbRobotDialog::refresh() { inRefreshRoutine=true; // Get handle of last selected object: int lastSel=simGetObjectLastSelection(); updateObjectsInList(); // Now initialized other dialog elements: CMtbRobot* taggedObj=NULL; ui->qqRobotProgram->setPlainText(""); ui->qqAutomatic->setChecked(false); if (lastSel!=-1) { taggedObj=CAccess::mtbRobotContainer->getFromAssociatedObject(lastSel); if (taggedObj==NULL) selectObjectInList(-1); else { // The last selected object is the base object of a MTB model! selectObjectInList(taggedObj->getID()); std::string prg(taggedObj->getProgram()); ui->qqRobotProgram->setPlainText(prg.c_str()); ui->qqAutomatic->setChecked((taggedObj->getOptions()&1)!=0); } } else selectObjectInList(-1); ui->qqRobotProgram->setEnabled((taggedObj!=NULL)&&(simGetSimulationState()==sim_simulation_stopped)); ui->qqAutomatic->setEnabled((taggedObj!=NULL)&&(simGetSimulationState()==sim_simulation_stopped)); inRefreshRoutine=false; }
void CBubbleRobDialog::refresh() { // This refreshed the dialog's items: int lastSel=simGetObjectLastSelection(); m_bubbleRobList.ResetContent(); for (int i=0;i<CAccess::bubbleRobContainer->getCount();i++) { CBubbleRob* bubbleRob=CAccess::bubbleRobContainer->getFromIndex(i); int sceneObjectHandle=bubbleRob->getAssociatedObject(); std::string txt="Object: "; char* name=NULL; if (sceneObjectHandle!=-1) name=simGetObjectName(sceneObjectHandle); if (name==NULL) txt+="Unassociated"; else { txt+="Associated with "; txt+=name; simReleaseBuffer(name); } int index=m_bubbleRobList.AddString(txt.c_str()); m_bubbleRobList.SetItemData(index,(DWORD)bubbleRob->getID()); } CBubbleRob* taggedObj=NULL; if (lastSel!=-1) { taggedObj=CAccess::bubbleRobContainer->getFromAssociatedObject(lastSel); if (taggedObj==NULL) selectObjectInList(-1); else selectObjectInList(taggedObj->getID()); } else selectObjectInList(-1); taggedObj=CAccess::bubbleRobContainer->getFromID(getSelectedObjectInList()); m_maxVelocity.EnableWindow(taggedObj!=NULL); if (taggedObj!=NULL) { CString tmp; tmp.Format("%0.2f",120.0f*taggedObj->getMaxVelocity()/3.1415f); // display it in RPM m_maxVelocity.SetWindowText(tmp); } else m_maxVelocity.SetWindowText(""); }
void CBubbleRobDialog::OnEnKillfocusMaxVelocity() { simLockInterface(1); CBubbleRob* bubbleRob=CAccess::bubbleRobContainer->getFromAssociatedObject(simGetObjectLastSelection()); if (bubbleRob!=NULL) { CString tmp; GetDlgItemText(IDC_MAX_VELOCITY,tmp); char* last; float v=float(strtod(tmp,&last)); if (last[0]==0) { bubbleRob->setMaxVelocity(3.1415f*v/120.0f); // we had it displayed in RPM simAnnounceSceneContentChange(); // this is so that V-REP registers a new undo-point here } } refresh(); simLockInterface(0); }
//Write void urdfLink::createLink(bool hideCollisionLinks,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool& showConvexDecompositionDlg) { std::string txt("Creating link '"+name+"'..."); printToConsole(txt.c_str()); //visuals.clear(); // Visuals for (int i=0; i<visuals.size(); i++) { urdfElement &visual = visuals[i]; if(!visual.meshFilename.empty()) { std::string fname(visual.meshFilename); bool exists=true; bool useAlt=false; if (!simDoesFileExist(fname.c_str())) { fname=visual.meshFilename_alt; exists=simDoesFileExist(fname.c_str()); useAlt=true; } if (!exists) printToConsole("ERROR: the mesh file could not be found."); else visual.n = simImportShape(visual.meshExtension,fname.c_str(),0,0.0001f,1.0); if (!visual.n) { if (!useAlt) txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension); else txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' or '"+visual.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension); printToConsole(txt.c_str()); } else visual.n = scaleShapeIfRequired(visual.n,visual.mesh_scaling); } else if (!isArrayEmpty(visual.sphere_size)) visual.n = simCreatePureShape( 1,1+2+16, visual.sphere_size, mass, NULL); else if (!isArrayEmpty(visual.cylinder_size)) visual.n = simCreatePureShape( 2,1+2+16, visual.cylinder_size, mass, NULL); else if (!isArrayEmpty(visual.box_size)) visual.n = simCreatePureShape( 0,1+2+16, visual.box_size, mass, NULL); } //collisions.clear(); //mass=0.1; //collision for (int i=0; i<collisions.size(); i++) { urdfElement &collision = collisions[i]; if(!collision.meshFilename.empty()) { std::string fname(collision.meshFilename); bool exists=true; bool useAlt=false; if (!simDoesFileExist(fname.c_str())) { fname=collision.meshFilename_alt; exists=simDoesFileExist(fname.c_str()); useAlt=true; } if (!exists) printToConsole("ERROR: the mesh file could not be found"); else collision.n = simImportShape(collision.meshExtension,fname.c_str(),0,0.0001f,1.0); if (collision.n == -1) { if (!useAlt) txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension); else txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' or '"+collision.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension); printToConsole(txt.c_str()); } else { collision.n=scaleShapeIfRequired(collision.n,collision.mesh_scaling); if (createVisualIfNone&&(visuals.size()==0)) { // We create a visual from the collision shape (before it gets morphed hereafter): simRemoveObjectFromSelection(sim_handle_all,-1); simAddObjectToSelection(sim_handle_single,collision.n); simCopyPasteSelectedObjects(); addVisual(); currentVisual().n = simGetObjectLastSelection(); } int p; int convInts[5]={1,500,200,0,0}; // 3rd value from 100 to 500 on 5/2/2014 float convFloats[5]={100.0f,30.0f,0.25f,0.0f,0.0f}; if ( convexDecomposeNonConvexCollidables&&(simGetObjectIntParameter(collision.n,3017,&p)>0)&&(p==0) ) { int aux=1+4+8+16+64; if (showConvexDecompositionDlg) aux=1+2+8+16+64; showConvexDecompositionDlg=false; simConvexDecompose(collision.n,aux,convInts,convFloats); // we generate convex shapes! } simSetObjectIntParameter(collision.n,3003,!inertiaPresent); // we make it non-static if there is an inertia simSetObjectIntParameter(collision.n,3004,1); // we make it respondable since it is a collision object } } else if (!isArrayEmpty(collision.sphere_size)) collision.n = simCreatePureShape( 1,1+2+4+8+16*(!inertiaPresent), collision.sphere_size, mass, NULL); else if (!isArrayEmpty(collision.cylinder_size)) collision.n = simCreatePureShape( 2,1+2+4+8+16*(!inertiaPresent), collision.cylinder_size, mass, NULL); else if (!isArrayEmpty(collision.box_size)) collision.n = simCreatePureShape( 0,1+2+4+8+16*(!inertiaPresent), collision.box_size, mass, NULL); } // Hack to draw COM in the collision layer /* addCollision(); currentCollision().xyz[0] = inertial_xyz[0]; currentCollision().xyz[1] = inertial_xyz[1]; currentCollision().xyz[0] = inertial_xyz[2]; currentCollision().rpy[0] = 1.5; float dummySize[3]={0.01f,0.01f,0.01f}; currentCollision().n = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL); */ // Grouping collisions shapes nLinkCollision = groupShapes(collisions); // Inertia if (inertiaPresent) { C3Vector euler; if (nLinkCollision==-1) { // we do not have a collision object. Let's create a dummy collision object, since inertias can't exist on their own in V-REP: float dummySize[3]={0.01f,0.01f,0.01f}; //nLinkCollision = simCreatePureShape( 1,1+2+4, dummySize, mass, NULL); // we make it non-respondable! nLinkCollision = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL); } C7Vector inertiaFrame; inertiaFrame.X.set(inertial_xyz); inertiaFrame.Q=getQuaternionFromRpy(inertial_rpy); //simSetObjectPosition(nLinkCollision,-1,inertiaFrame.X.data); //C7Vector collisionFrame; //collisionFrame.X.set(collision_xyz); //collisionFrame.Q=getQuaternionFromRpy(collision_rpy); C7Vector collisionFrame; simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data); simGetObjectOrientation(nLinkCollision,-1,euler.data); collisionFrame.Q.setEulerAngles(euler); //C4X4Matrix x((collisionFrame.getInverse()*inertiaFrame).getMatrix()); C4X4Matrix x(inertiaFrame.getMatrix()); float i[12]={x.M(0,0),x.M(0,1),x.M(0,2),x.X(0),x.M(1,0),x.M(1,1),x.M(1,2),x.X(1),x.M(2,0),x.M(2,1),x.M(2,2),x.X(2)}; simSetShapeMassAndInertia(nLinkCollision,mass,inertia,C3Vector::zeroVector.data,i); //std::cout << "Mass: " << mass << std::endl; } else { if (nLinkCollision!=-1) { std::string txt("ERROR: found a collision object without inertia data for link '"+ name+"'. Is that link meant to be static?"); printToConsole(txt.c_str()); } } if (createVisualIfNone&&(visuals.size()==0)&&(nLinkCollision!=-1)) { // We create a visual from the collision shape (meshes were handled earlier): addVisual(); urdfElement &visual = currentVisual(); simRemoveObjectFromSelection(sim_handle_all,-1); simAddObjectToSelection(sim_handle_single,nLinkCollision); simCopyPasteSelectedObjects(); visual.n=simGetObjectLastSelection(); simSetObjectIntParameter(visual.n,3003,1); // we make it static since only visual simSetObjectIntParameter(visual.n,3004,0); // we make it non-respondable since only visual } // Set the respondable mask: if (nLinkCollision!=-1) simSetObjectIntParameter(nLinkCollision,3019,0xff00); // colliding with everything except with other objects in that tree hierarchy // Grouping shapes nLinkVisual = groupShapes(visuals); // Set the names, visibility, etc.: if (nLinkVisual!=-1) { setVrepObjectName(nLinkVisual,std::string(name+"_visual").c_str()); const float specularDiffuse[3]={0.3f,0.3f,0.3f}; if (nLinkCollision!=-1) { // if we have a collision object, we attach the visual object to it, then forget the visual object C7Vector collisionFrame; C3Vector euler; simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data); simGetObjectOrientation(nLinkCollision,-1,euler.data); collisionFrame.Q.setEulerAngles(euler); C7Vector visualFrame; simGetObjectPosition(nLinkVisual,-1,visualFrame.X.data); simGetObjectOrientation(nLinkVisual,-1,euler.data); visualFrame.Q.setEulerAngles(euler); C7Vector x(collisionFrame.getInverse()*visualFrame); simSetObjectPosition(nLinkVisual,-1,x.X.data); simSetObjectOrientation(nLinkVisual,-1,x.Q.getEulerAngles().data); simSetObjectParent(nLinkVisual,nLinkCollision,0); } } if (nLinkCollision!=-1) { setVrepObjectName(nLinkCollision,std::string(name+"_respondable").c_str()); if (hideCollisionLinks) simSetObjectIntParameter(nLinkCollision,10,256); // we "hide" that object in layer 9 } }