DictionaryTrajectory::DictionaryTrajectory(std::string baseFolder, double az, double bz) : Trajectory() { this->baseFolder = baseFolder; vector<string> files = getFilesInDirectory(baseFolder); queryFiles = sortPrefix(files, "query"); trajFiles = sortPrefix(files, "traj"); dmpFiles = sortPrefix(files, "dmp"); if(dmpFiles.size() == 0) { // learn dmps queryPoints = mapFiles(queryFiles, trajFiles, "query", "traj"); KUKADU_SHARED_PTR<JointDMPLearner> dmpLearner; vector<mat> jointsVec; double tMax = 0.0; for(int i = 0; i < queryPoints.size(); ++i) { mat joints = readMovements((string(baseFolder) + string(queryPoints.at(i).getFileDataPath())).c_str()); degOfFreedom = joints.n_cols - 1; queryPoints.at(i).setQueryPoint(readQuery(string(baseFolder) + string(queryPoints.at(i).getFileQueryPath()))); jointsVec.push_back(joints); double currentTMax = joints(joints.n_rows - 1, 0); if(tMax < currentTMax) tMax = currentTMax; } for(int i = 0; i < jointsVec.size(); ++i) { QueryPoint currentQueryPoint = queryPoints.at(i); mat joints = jointsVec.at(i); joints = fillTrajectoryMatrix(joints, tMax); dmpLearner = KUKADU_SHARED_PTR<JointDMPLearner>(new JointDMPLearner(az, bz, joints)); KUKADU_SHARED_PTR<Dmp> learnedDmps = dmpLearner->fitTrajectories(); learnedDmps->serialize(baseFolder + currentQueryPoint.getFileDmpPath()); queryPoints.at(i).setDmp(learnedDmps); startingPos = queryPoints.at(i).getDmp()->getY0(); cout << "(DMPGeneralizer) goals for query point [" << currentQueryPoint.getQueryPoint().t() << "]" << endl << "\t ["; cout << currentQueryPoint.getDmp()->getG().t() << "]" << endl; //delete dmpLearner; dmpLearner = KUKADU_SHARED_PTR<JointDMPLearner>(); } } else { queryPoints = mapFiles(queryFiles, trajFiles, dmpFiles, "query", "traj", "dmp"); } degOfFreedom = queryPoints.at(0).getDmp()->getDegreesOfFreedom(); }
void PR2EihMapping::_return_grasp_joint_traj_callback(const trajectory_msgs::JointTrajectoryConstPtr& msg) { return_grasp_joint_traj.clear(); for(const trajectory_msgs::JointTrajectoryPoint& jt_pt : msg->points) { VectorJ joints(jt_pt.positions.data()); return_grasp_joint_traj.push_back(joints); } }
/* ============ TestUntransformJoints ============ */ void TestUntransformJoints() { int i, j; TIME_TYPE start, end, bestClocksGeneric, bestClocksSIMD; idTempArray< idJointMat > joints( COUNT+1 ); idTempArray< idJointMat > joints1( COUNT+1 ); idTempArray< idJointMat > joints2( COUNT+1 ); idTempArray< int > parents( COUNT+1 ); const char *result; idRandom srnd( RANDOM_SEED ); for ( i = 0; i <= COUNT; i++ ) { idAngles angles; angles[0] = srnd.CRandomFloat() * 180.0f; angles[1] = srnd.CRandomFloat() * 180.0f; angles[2] = srnd.CRandomFloat() * 180.0f; joints[i].SetRotation( angles.ToMat3() ); idVec3 v; v[0] = srnd.CRandomFloat() * 2.0f; v[1] = srnd.CRandomFloat() * 2.0f; v[2] = srnd.CRandomFloat() * 2.0f; joints[i].SetTranslation( v ); parents[i] = i - 1; } bestClocksGeneric = 0; for ( i = 0; i < NUMTESTS; i++ ) { for ( j = 0; j <= COUNT; j++ ) { joints1[j] = joints[j]; } StartRecordTime( start ); p_generic->UntransformJoints( joints1.Ptr(), parents.Ptr(), 1, COUNT ); StopRecordTime( end ); GetBest( start, end, bestClocksGeneric ); } PrintClocks( "generic->UntransformJoints()", COUNT, bestClocksGeneric ); bestClocksSIMD = 0; for ( i = 0; i < NUMTESTS; i++ ) { for ( j = 0; j <= COUNT; j++ ) { joints2[j] = joints[j]; } StartRecordTime( start ); p_simd->UntransformJoints( joints2.Ptr(), parents.Ptr(), 1, COUNT ); StopRecordTime( end ); GetBest( start, end, bestClocksSIMD ); } for ( i = 1; i <= COUNT; i++ ) { if ( !joints1[i].Compare( joints2[i], 1e-3f ) ) { break; } } result = ( i >= COUNT ) ? "ok" : S_COLOR_RED"X"; PrintClocks( va( " simd->UntransformJoints() %s", result ), COUNT, bestClocksSIMD, bestClocksGeneric ); }
void ArmatureExporter::add_joints_element(ListBase *defbase, const std::string& joints_source_id, const std::string& inv_bind_mat_source_id) { COLLADASW::JointsElement joints(mSW); COLLADASW::InputList &input = joints.getInputList(); input.push_back(COLLADASW::Input(COLLADASW::InputSemantic::JOINT, // constant declared in COLLADASWInputList.h COLLADASW::URI(COLLADABU::Utils::EMPTY_STRING, joints_source_id))); input.push_back(COLLADASW::Input(COLLADASW::InputSemantic::BINDMATRIX, COLLADASW::URI(COLLADABU::Utils::EMPTY_STRING, inv_bind_mat_source_id))); joints.add(); }
GeneralDmpLearner::GeneralDmpLearner(vector<double> mysDef, vector<double> sigmasDef, double az, double bz, string file) { // reading in file mat joints = readMovements(file); double tau = joints(joints.n_rows - 1, 0); double ax = -std::log(0.1) / tau / tau; int degFreedom = joints.n_cols - 1; vector<DMPBase> baseDef = buildDMPBase(mysDef, sigmasDef, ax, tau); this->construct(baseDef, tau, az, bz, ax, joints, degFreedom); }
GeneralDmpLearner::GeneralDmpLearner(double az, double bz, std::string file) { vector<double> tmpmys; vector<double> tmpsigmas; tmpsigmas.push_back(0.2); tmpsigmas.push_back(0.8); mat joints = readMovements(file); int degFreedom = joints.n_cols - 1; tmpmys = constructDmpMys(joints); double tau = joints(joints.n_rows - 1, 0); double ax = -std::log(0.1) / tau / tau; vector<DMPBase> baseDef = buildDMPBase(tmpmys, tmpsigmas, ax, tau); this->construct(baseDef, tau, az, bz, ax, joints, degFreedom); }
void Pulley::finishComponent() { Vector3 wheel(getNode(1)->getTranslationWorld()); unsigned short i, j, v = 0; std::vector<MyNode*> links(_numLinks); std::vector<Vector3> joints(_numLinks+1); //create the chain by duplicating a box a bunch of times float x, y, z = wheel.z, angle; Vector3 zAxis(0, 0, 1), joint, trans1, trans2; joints[0].set(wheel); joints[0].x -= _radius; joints[0].y -= (_dropLinks + 0.5f) * _linkLength; MyNode *link; for(i = 0; i < _numLinks; i++) { link = app->duplicateModelNode("box"); std::stringstream ss; ss << _id << _typeCount << "_link" << (i+1); const std::string nodeID = ss.str(); link->setId(nodeID.c_str()); if(i < _dropLinks) { //chain going up from left bucket x = wheel.x - _radius; y = wheel.y - (_dropLinks-i) * _linkLength; angle = 0; } else if(i < _dropLinks + _wheelLinks+1) { //chain going over wheel angle = (i - _dropLinks) * (M_PI / _wheelLinks); x = wheel.x -_radius * cos(angle); y = wheel.y + _radius * sin(angle); } else { //chain going down to right bucket x = wheel.x + _radius; y = wheel.y - (_dropLinks - (_numLinks-1 - i)) * _linkLength; angle = M_PI; } link->setScale(_linkWidth/3, _linkLength/3, _linkWidth/3); link->rotate(zAxis, -angle); link->setTranslation(x, y, z); link->addCollisionObject(); links[i] = link; _rootNode->addChild(link); //note the position of the joint between this link and the next joint.set(0, (_linkLength/2) / link->getScaleY(), 0); link->getWorldMatrix().transformPoint(&joint); joints[i+1].set(joint); } //must enable all collision objects before adding constraints to prevent crashes for(i = 0; i < _elements.size(); i++) { getNode(i)->getCollisionObject()->setEnabled(true); } //connect each pair of adjacent links with a socket constraint PhysicsSocketConstraint *constraint; Quaternion rot1, rot2; for(i = 0; i < _numLinks-1; i++) { trans1.set(0, (_linkLength/2) / links[i]->getScaleY(), 0); trans2.set(0, -(_linkLength/2) / links[i]->getScaleY(), 0); constraint = (PhysicsSocketConstraint*) app->addConstraint(links[i], links[i+1], -1, "socket", joints[i+1], Vector3::unitZ()); _constraints.push_back(constraint); } //connect each bucket to the adjacent chain link with a socket constraint for(i = 0; i < 2; i++) { constraint = (PhysicsSocketConstraint*) app->addConstraint(links[i * (_numLinks-1)], getNode(i+2), -1, "socket", joints[i * _numLinks], Vector3::unitZ()); _constraints.push_back(constraint); } }
//--------------------------------------------------------------- void ControllerExporter::exportSkinController( ExportNode* exportNode, SkinController* skinController, const String& controllerId, const String& skinSource ) { INode* iNode = exportNode->getINode(); if ( !skinController ) return; // We cannot use skin->GetContextInterface to get ISkinContextData if we are exporting an XRef, since we cannot access // the INode the object belongs to in the referenced file. To solve this problem, we temporarily create an INode, that references // the object and delete it immediately. ISkinInterface* skinInterface = getISkinInterface( exportNode, skinController ); if ( !skinInterface ) return; openSkin(controllerId, skinSource); Matrix3 bindShapeTransformationMatrix; skinInterface->getSkinInitTM(bindShapeTransformationMatrix, true); double bindShapeTransformationArray[4][4]; VisualSceneExporter::matrix3ToDouble4x4(bindShapeTransformationArray, bindShapeTransformationMatrix); addBindShapeTransform(bindShapeTransformationArray); int jointCount = skinInterface->getNumBones(); INodeList boneINodes; // Export joints source String jointsId = controllerId + JOINTS_SOURCE_ID_SUFFIX; COLLADASW::NameSource jointSource(mSW); jointSource.setId(jointsId); jointSource.setArrayId(jointsId + ARRAY_ID_SUFFIX); jointSource.setAccessorStride(1); jointSource.getParameterNameList().push_back("JOINT"); jointSource.setAccessorCount(jointCount); jointSource.prepareToAppendValues(); for (int i = 0; i < jointCount; ++i) { // there should not be any null bone. // the ISkin::GetBone, not GetBoneFlat, function is called here. INode* boneNode = skinInterface->getBone(i); assert(boneNode); boneINodes.push_back(boneNode); ExportNode* jointExportNode = mExportSceneGraph->getExportNode(boneNode); assert(jointExportNode); if ( !jointExportNode->hasSid() ) jointExportNode->setSid(mExportSceneGraph->createJointSid()); jointExportNode->setIsJoint(); jointSource.appendValues(jointExportNode->getSid()); } jointSource.finish(); determineReferencedJoints(exportNode, skinController); //export inverse bind matrix source String inverseBindMatrixId = controllerId + BIND_POSES_SOURCE_ID_SUFFIX; COLLADASW::Float4x4Source inverseBindMatrixSource(mSW); inverseBindMatrixSource.setId(inverseBindMatrixId); inverseBindMatrixSource.setArrayId(inverseBindMatrixId + ARRAY_ID_SUFFIX); inverseBindMatrixSource.setAccessorStride(16); inverseBindMatrixSource.getParameterNameList().push_back("TRANSFORM"); inverseBindMatrixSource.setAccessorCount(jointCount); inverseBindMatrixSource.prepareToAppendValues(); for (int i = 0; i < jointCount; ++i) { INode* boneNode = boneINodes[i]; Matrix3 bindPose; if ( !skinInterface->getBoneInitTM(boneNode, bindPose) ) { bindPose = VisualSceneExporter::getWorldTransform( boneNode, mDocumentExporter->getOptions().getAnimationStart() ); } bindPose.Invert(); double bindPoseArray[4][4]; VisualSceneExporter::matrix3ToDouble4x4(bindPoseArray, bindPose); inverseBindMatrixSource.appendValues(bindPoseArray); } inverseBindMatrixSource.finish(); int vertexCount = skinInterface->getNumVertices(); //count weights, excluding the ones equals one int weightsCount = 1; for (int i = 0; i < vertexCount; ++i) { int jointCount = skinInterface->getNumAssignedBones(i); for (int p = 0; p < jointCount; ++p) { float weight = skinInterface->getBoneWeight(i, p); if ( !COLLADASW::MathUtils::equals(weight, 1.0f) ) weightsCount++; } } //export weights source String weightsId = controllerId + WEIGHTS_SOURCE_ID_SUFFIX; COLLADASW::FloatSource weightsSource(mSW); weightsSource.setId(weightsId); weightsSource.setArrayId(weightsId + ARRAY_ID_SUFFIX); weightsSource.setAccessorStride(1); weightsSource.getParameterNameList().push_back("WEIGHT"); weightsSource.setAccessorCount(weightsCount); weightsSource.prepareToAppendValues(); weightsSource.appendValues(1.0); for (int i = 0; i < vertexCount; ++i) { int jointCount = skinInterface->getNumAssignedBones(i); for (int p = 0; p < jointCount; ++p) { float weight = skinInterface->getBoneWeight(i, p); if ( !COLLADASW::MathUtils::equals(weight, 1.0f) ) weightsSource.appendValues(weight); } } weightsSource.finish(); COLLADASW::JointsElement joints(mSW); joints.getInputList().push_back(COLLADASW::Input(COLLADASW::InputSemantic::JOINT, "#" + jointsId)); joints.getInputList().push_back(COLLADASW::Input(COLLADASW::InputSemantic::BINDMATRIX, "#" + inverseBindMatrixId)); joints.add(); COLLADASW::VertexWeightsElement vertexWeights(mSW); COLLADASW::Input weightInput(COLLADASW::InputSemantic::WEIGHT, "#" + weightsId); vertexWeights.getInputList().push_back(COLLADASW::Input(COLLADASW::InputSemantic::JOINT, "#" + jointsId, 0)); vertexWeights.getInputList().push_back(COLLADASW::Input(COLLADASW::InputSemantic::WEIGHT, "#" + weightsId, 1)); vertexWeights.setCount(vertexCount); vertexWeights.prepareToAppendVCountValues(); for (int i = 0; i < vertexCount; ++i) vertexWeights.appendValues(skinInterface->getNumAssignedBones(i)); vertexWeights.CloseVCountAndOpenVElement(); int currentIndex = 1; for (int i = 0; i < vertexCount; ++i) { int jointCount = skinInterface->getNumAssignedBones(i); for (int p = 0; p < jointCount; ++p) { vertexWeights.appendValues(skinInterface->getAssignedBone(i, p)); float weight = skinInterface->getBoneWeight(i, p); if ( !COLLADASW::MathUtils::equals(weight, 1.0f) ) { vertexWeights.appendValues(currentIndex++); } else { vertexWeights.appendValues(0); } } } vertexWeights.finish(); closeSkin(); skinInterface->releaseMe(); }
void Tool::setUpModules() { diagram.connectToUnlogger<messages::YUVImage>(topConverter.imageIn, "top"); diagram.connectToUnlogger<messages::YUVImage>(bottomConverter.imageIn, "bottom"); diagram.connectToUnlogger<messages::YUVImage>(topConverter.imageIn, "top"); diagram.connectToUnlogger<messages::YUVImage>(bottomConverter.imageIn, "bottom"); diagram.addModule(topConverter); diagram.addModule(bottomConverter); topConverter.loadTable(globalColorTable.getTable()); bottomConverter.loadTable(globalColorTable.getTable()); diagram.addModule(visDispMod); diagram.connectToUnlogger<messages::YUVImage>(visDispMod.topImageIn, "top"); diagram.connectToUnlogger<messages::YUVImage>(visDispMod.bottomImageIn, "bottom"); if (diagram.connectToUnlogger<messages::JointAngles>(visDispMod.joints_in, "joints") && diagram.connectToUnlogger<messages::InertialState>(visDispMod.inerts_in, "inertials")) { // All is well } else { std::cout << "Warning: Joints and Inertials not logged in this file.\n"; portals::Message<messages::JointAngles> joints(0); portals::Message<messages::InertialState> inertials(0); visDispMod.joints_in.setMessage(joints); visDispMod.inerts_in.setMessage(inertials); } visDispMod.tTImage_in.wireTo(&topConverter.thrImage, true); visDispMod.tYImage_in.wireTo(&topConverter.yImage, true); visDispMod.tUImage_in.wireTo(&topConverter.uImage, true); visDispMod.tVImage_in.wireTo(&topConverter.vImage, true); visDispMod.bTImage_in.wireTo(&bottomConverter.thrImage, true); visDispMod.bYImage_in.wireTo(&bottomConverter.yImage, true); visDispMod.bUImage_in.wireTo(&bottomConverter.uImage, true); visDispMod.bVImage_in.wireTo(&bottomConverter.vImage, true); /** Color Table Creator Tab **/ if (diagram.connectToUnlogger<messages::YUVImage>(tableCreator.topImageIn, "top") && diagram.connectToUnlogger<messages::YUVImage>(tableCreator.bottomImageIn, "bottom")) { diagram.addModule(tableCreator); tableCreator.topThrIn.wireTo(&topConverter.thrImage, true); tableCreator.botThrIn.wireTo(&bottomConverter.thrImage, true); } else { std::cout << "Right now you can't use the color table creator without" << " two image logs." << std::endl; } /** Color Calibrate Tab **/ if (diagram.connectToUnlogger<messages::YUVImage>(colorCalibrate.topImageIn, "top") && diagram.connectToUnlogger<messages::YUVImage>(colorCalibrate.bottomImageIn, "bottom")) { diagram.addModule(colorCalibrate); } else { std::cout << "Right now you can't use the color calibrator without" << " two image logs." << std::endl; } /** FieldViewer Tab **/ // Should add field view bool shouldAddFieldView = false; if(diagram.connectToUnlogger<messages::RobotLocation>(fieldView.locationIn, "location")) { fieldView.confirmLocationLogs(true); shouldAddFieldView = true; } else { std::cout << "Warning: location wasn't logged in this file" << std::endl; } if(diagram.connectToUnlogger<messages::RobotLocation>(fieldView.odometryIn, "odometry")) { fieldView.confirmOdometryLogs(true); shouldAddFieldView = true; } else { std::cout << "Warning: odometry wasn't logged in this file" << std::endl; } if(diagram.connectToUnlogger<messages::ParticleSwarm>(fieldView.particlesIn, "particleSwarm")) { fieldView.confirmParticleLogs(true); shouldAddFieldView = true; } else { std::cout << "Warning: Particles weren't logged in this file" << std::endl; } if(diagram.connectToUnlogger<messages::VisionField>(fieldView.observationsIn, "observations")) { fieldView.confirmObsvLogs(true); shouldAddFieldView = true; } else { std::cout << "Warning: Observations weren't logged in this file" << std::endl; } if(shouldAddFieldView) diagram.addModule(fieldView); }
virtual bool create(piecewise_surface_type &ps) const { typedef typename piecewise_surface_type::surface_type surface_type; index_type nribs(this->get_number_u_segments()+1), i, j; std::vector<index_type> seg_degree(nribs-1); std::vector<rib_data_type> rib_states(ribs); tolerance_type tol; // FIX: Should be able to handle closed surfaces assert(!closed); // FIX: Need to be able to handle v-direction discontinuous fu and fuu specifications // reset the incoming piecewise surface ps.clear(); // split ribs so have same number of curves (with same joint parameters) for all ribs and get degree index_type njoints(this->get_number_v_segments()+1); std::vector<data_type> joints(njoints); std::vector<index_type> max_jdegs(njoints-1,0); joints[0]=this->get_v0(); for (j=0; j<(njoints-1); ++j) { joints[j+1]=joints[j]+this->get_segment_dv(j); } for (i=0; i<nribs; ++i) { std::vector<index_type> jdegs; rib_states[i].split(joints.begin(), joints.end(), std::back_inserter(jdegs)); for (j=0; j<(njoints-1); ++j) { if (jdegs[j]>max_jdegs[j]) { max_jdegs[j]=jdegs[j]; } } } // set degree in u-direction for each rib segment strip for (i=0; i<nribs; ++i) { rib_states[i].promote(max_jdegs.begin(), max_jdegs.end()); } // resize the piecewise surface index_type u, v, nu(nribs-1), nv(njoints-1); ps.init_uv(this->du_begin(), this->du_end(), this->dv_begin(), this->dv_end(), this->get_u0(), this->get_v0()); // build segments based on rib information // here should have everything to make an nribs x njoints piecewise surface with all // of the j-degrees matching in the u-direction so that can use general curve creator // techniques to create control points for (v=0; v<nv; ++v) { typedef eli::geom::curve::piecewise_general_creator<data_type, dim__, tolerance_type> piecewise_curve_creator_type; typedef eli::geom::curve::piecewise<eli::geom::curve::bezier, data_type, dim__, tolerance_type> piecewise_curve_type; typedef typename piecewise_curve_type::curve_type curve_type; std::vector<typename piecewise_curve_creator_type::joint_data> joints(nu+1); piecewise_curve_creator_type gc; piecewise_curve_type c; std::vector<surface_type> surfs(nu); for (j=0; j<=max_jdegs[v]; ++j) { // cycle through each rib to set corresponding joint info for (u=0; u<=nu; ++u) { curve_type jcrv; joints[u].set_continuity(static_cast<typename piecewise_curve_creator_type::joint_continuity>(rib_states[u].get_continuity())); rib_states[u].get_f().get(jcrv, v); joints[u].set_f(jcrv.get_control_point(j)); if (rib_states[u].use_left_fp()) { rib_states[u].get_left_fp().get(jcrv, v); joints[u].set_left_fp(jcrv.get_control_point(j)); } if (rib_states[u].use_right_fp()) { rib_states[u].get_right_fp().get(jcrv, v); joints[u].set_right_fp(jcrv.get_control_point(j)); } if (rib_states[u].use_left_fpp()) { rib_states[u].get_left_fpp().get(jcrv, v); joints[u].set_left_fpp(jcrv.get_control_point(j)); } if (rib_states[u].use_right_fpp()) { rib_states[u].get_right_fpp().get(jcrv, v); joints[u].set_right_fpp(jcrv.get_control_point(j)); } } // set the conditions for the curve creator bool rtn_flag(gc.set_conditions(joints, max_degree, closed)); if (!rtn_flag) { return false; } // set the parameterizations and create curve gc.set_t0(this->get_u0()); for (u=0; u<nu; ++u) { gc.set_segment_dt(this->get_segment_du(u), u); } rtn_flag=gc.create(c); if (!rtn_flag) { return false; } // extract the control points from piecewise curve and set the surface control points for (u=0; u<nu; ++u) { curve_type crv; c.get(crv, u); // resize the temp surface if (j==0) { surfs[u].resize(crv.degree(), max_jdegs[v]); } for (i=0; i<=crv.degree(); ++i) { surfs[u].set_control_point(crv.get_control_point(i), i, j); } } } // put these surfaces into piecewise surface typename piecewise_surface_type::error_code ec; for (u=0; u<nu; ++u) { ec=ps.set(surfs[u], u, v); if (ec!=piecewise_surface_type::NO_ERRORS) { assert(false); return false; } } } return true; }