void CSubscriberData::setObjectFloatParameterCallback(const std_msgs::Float32::ConstPtr& param) { if (_handleGeneralCallback_before()) { if (simSetObjectFloatParameter(auxInt1,auxInt2,param->data)<=0) shutDownGeneralSubscriber(); _handleGeneralCallback_after(); } }
void SetTwistHandler::handleSimulation(){ // called when the main script calls: simHandleModule if(!_initialized){ _initialize(); } Eigen::Quaternion < simFloat > orientation; //(x,y,z,w) Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x, (simFloat)_twistCommands.twist.linear.y, (simFloat)_twistCommands.twist.linear.z); Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x, (simFloat)_twistCommands.twist.angular.y, (simFloat)_twistCommands.twist.angular.z); // Input velocity is expected to be in body frame but V-Rep expects it to be in world frame. if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){ linVelocity = orientation*linVelocity; angVelocity = orientation*angVelocity; } else { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl; ConsoleHandler::printInConsole(ss); } simResetDynamicObject(_associatedObjectID); //Set object velocity if (_isStatic){ Eigen::Matrix<simFloat, 3, 1> position; simGetObjectPosition(_associatedObjectID, -1, position.data()); const simFloat timeStep = simGetSimulationTimeStep(); position = position + timeStep*linVelocity; simSetObjectPosition(_associatedObjectID, -1, position.data()); const simFloat angle = timeStep*angVelocity.norm(); if(angle > 1e-6){ orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation; } simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()); } else { // Apply the linear velocity to the object if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0]) && simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1]) && simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;; ConsoleHandler::printInConsole(ss); } // Apply the angular velocity to the object if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0]) && simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1]) && simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) { std::stringstream ss; ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;; ConsoleHandler::printInConsole(ss); } } }
void robot::createJoints(bool hideJoints,bool positionCtrl) { std::string txt("There are "+boost::lexical_cast<std::string>(vJoints.size())+" joints."); printToConsole(txt.c_str()); //Set parents and childs for all the links for(size_t i = 0; i < vJoints.size() ; i++) { vLinks.at(getLinkPosition(vJoints.at(i)->parentLink))->child = vJoints.at(i)->name; vLinks.at(getLinkPosition(vJoints.at(i)->childLink))->parent = vJoints.at(i)->name; } //Create the joints for(size_t i = 0; i < vJoints.size() ; i++) { //Move the joints to the positions specifieds by the urdf file C7Vector tmp; tmp.setIdentity(); tmp.X.set(vJoints.at(i)->origin_xyz); tmp.Q=getQuaternionFromRpy(vJoints.at(i)->origin_rpy); vJoints.at(i)->jointBaseFrame=vJoints.at(i)->jointBaseFrame*tmp; //Set name jointParent to each joint int nParentLink = getLinkPosition(vJoints.at(i)->parentLink); vJoints.at(i)->parentJoint = vLinks.at(nParentLink)->parent; //Create the joint/forceSensor/dummy: if (vJoints.at(i)->jointType==-1) vJoints.at(i)->nJoint = simCreateDummy(0.02f,NULL); // when joint type was not recognized if (vJoints.at(i)->jointType==0) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==1) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_prismatic_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==2) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_spherical_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==3) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==4) { // when joint type is "fixed" int intParams[5]={1,4,4,0,0}; float floatParams[5]={0.02f,1.0f,1.0f,0.0f,0.0f}; vJoints.at(i)->nJoint = simCreateForceSensor(0,intParams,floatParams,NULL); } if ( (vJoints.at(i)->jointType==0)||(vJoints.at(i)->jointType==1) ) { float interval[2]={vJoints.at(i)->lowerLimit,vJoints.at(i)->upperLimit-vJoints.at(i)->lowerLimit}; simSetJointInterval(vJoints.at(i)->nJoint,0,interval); if (vJoints.at(i)->jointType==0) { // revolute simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitAngular); simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitAngular); } else { // prismatic simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitLinear); simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitLinear); } // We turn the position control on: if (positionCtrl) { simSetObjectIntParameter(vJoints.at(i)->nJoint,2000,1); simSetObjectIntParameter(vJoints.at(i)->nJoint,2001,1); } } //Set the name: setVrepObjectName(vJoints.at(i)->nJoint,vJoints.at(i)->name.c_str()); if (hideJoints) simSetObjectIntParameter(vJoints.at(i)->nJoint,10,512); // layer 10 } //Set positions to joints from the 4x4matrix for(size_t i = 0; i < vJoints.size() ; i++) { simSetObjectPosition(vJoints.at(i)->nJoint,-1,vJoints.at(i)->jointBaseFrame.X.data); simSetObjectOrientation(vJoints.at(i)->nJoint,-1 ,vJoints.at(i)->jointBaseFrame.Q.getEulerAngles().data); } //Set joint parentship between them (thes parentship will be remove before adding the joints) for(size_t i = 0; i < vJoints.size() ; i++) { int parentJointIndex=getJointPosition(vJoints.at(i)->parentJoint); if ( parentJointIndex!= -1) { simInt nParentJoint = vJoints.at(parentJointIndex)->nJoint; simInt nJoint = vJoints.at(i)->nJoint; simSetObjectParent(nJoint,nParentJoint,false); } } //Delete all the partnership without moving the joints but after doing that update the transform matrix for(size_t i = 0; i < vJoints.size() ; i++) { C4X4Matrix tmp; simGetObjectPosition(vJoints.at(i)->nJoint,-1,tmp.X.data); C3Vector euler; simGetObjectOrientation(vJoints.at(i)->nJoint,-1,euler.data); tmp.M.setEulerAngles(euler); vJoints.at(i)->jointBaseFrame = tmp; simInt nJoint = vJoints.at(i)->nJoint; simSetObjectParent(nJoint,-1,true); } for(size_t i = 0; i < vJoints.size() ; i++) { C4X4Matrix jointAxisMatrix; jointAxisMatrix.setIdentity(); C3Vector axis(vJoints.at(i)->axis); C3Vector rotAxis; float rotAngle=0.0f; if (axis(2)<1.0f) { if (axis(2)<=-1.0f) rotAngle=3.14159265359f; else rotAngle=acosf(axis(2)); rotAxis(0)=-axis(1); rotAxis(1)=axis(0); rotAxis(2)=0.0f; rotAxis.normalize(); C7Vector m(jointAxisMatrix); float alpha=-atan2(rotAxis(1),rotAxis(0)); float beta=atan2(-sqrt(rotAxis(0)*rotAxis(0)+rotAxis(1)*rotAxis(1)),rotAxis(2)); C7Vector r; r.X.clear(); r.Q.setEulerAngles(0.0f,0.0f,alpha); m=r*m; r.Q.setEulerAngles(0.0f,beta,0.0f); m=r*m; r.Q.setEulerAngles(0.0f,0.0f,rotAngle); m=r*m; r.Q.setEulerAngles(0.0f,-beta,0.0f); m=r*m; r.Q.setEulerAngles(0.0f,0.0f,-alpha); m=r*m; jointAxisMatrix=m.getMatrix(); } C4Vector q((vJoints.at(i)->jointBaseFrame*jointAxisMatrix).Q); simSetObjectOrientation(vJoints.at(i)->nJoint,-1,q.getEulerAngles().data); } }