/* Helper function that sets the joint's base position * This places the kinematic chain at the appropriate position around the base of the hand. * * @param h - Pointer to the hand being created * @param fingerNumber - The index of the kinematic chain being added. */ void EigenHandLoader::setBaseJointPosition(Hand * h, unsigned int fingerNumber) { // Get base joint Joint * j = h->getChain(fingerNumber)->getJoint(0); // Calculate offset angle from database joint description double offsetAngle = hd->fingerBasePositions[fingerNumber]*M_PI/180.0; // Set the offset angle of the joint j->setOffset(offsetAngle); // Disable collisions between the palm and the first link. h->getWorld()->toggleCollisions(false, h->getChain(fingerNumber)->getLink(1), h->getPalm()); // These calculations are probably obsolete. //j->setMin(j->getMin()+offsetAngle); //if(j->getMin() > M_PI) // j->setMin(j->getMin() - 2*M_PI); //j->setMax(j->getMax() + offsetAngle); //if(j->getMax() > M_PI) // j->setMax(j->getMax() - 2*M_PI); // Set the current joint position to 0.0, given the new offset j->setVal(0.0); }
void BodyNode::updateBodyForce(const Eigen::Vector3d& _gravity, bool _withExternalForces) { if (mGravityMode == true) mFgravity.noalias() = mI * math::AdInvRLinear(mW, _gravity); else mFgravity.setZero(); mF.noalias() = mI * mdV; // Inertial force if (_withExternalForces) mF -= mFext; // External force for (int i = 0; i < mContactForces.size(); ++i) mF -= mContactForces[i]; assert(!math::isNan(mF)); mF -= mFgravity; // Gravity force mF -= math::dad(mV, mI * mV); // Coriolis force for (std::vector<BodyNode*>::iterator iChildBody = mChildBodyNodes.begin(); iChildBody != mChildBodyNodes.end(); ++iChildBody) { Joint* childJoint = (*iChildBody)->getParentJoint(); assert(childJoint != NULL); mF += math::dAdInvT(childJoint->getLocalTransform(), (*iChildBody)->getBodyForce()); } assert(!math::isNan(mF)); }
//Create new joint and place it in the middle of screen void DrawingWindow::createJoint() { Joint *joint = new Joint; joint->setPos(500,500); joints << joint; drawingScene->addItem(joint); }
shared_ptr<Joint> Joint::GetJoint(dJointID id) { if (id == 0) { return shared_ptr<Joint>(); } Joint* jointPtr = static_cast<Joint*>(dJointGetData(id)); if (jointPtr == 0) { // we cannot use the logserver here cerr << "ERROR: (Joint) no joint found for dJointID " << id << "\n"; return shared_ptr<Joint>(); } shared_ptr<Joint> joint = shared_static_cast<Joint> (jointPtr->GetSelf().lock()); if (joint.get() == 0) { // we cannot use the logserver here cerr << "ERROR: (Joint) got no shared_ptr for dJointID " << id << "\n"; } return joint; }
boost::shared_ptr<Joint> Joint::GetJoint(long jointID) { if (jointID == 0) { return boost::shared_ptr<Joint>(); } Joint* jointPtr = mJointImp->GetJoint(jointID); if (jointPtr == 0) { // we cannot use the logserver here cerr << "ERROR: (Joint) no joint found for dJointID " << jointID << "\n"; return boost::shared_ptr<Joint>(); } boost::shared_ptr<Joint> joint = static_pointer_cast<Joint> (jointPtr->GetSelf().lock()); if (joint.get() == 0) { // we cannot use the logserver here cerr << "ERROR: (Joint) got no boost::shared_ptr for dJointID " << jointID << "\n"; } return joint; }
/** HACK! */ void Character::updateJointOrdering(){ if( jointOrder.size() == joints.size() ) return; // HACK assume ordering is ok jointOrder.clear(); if (!root) return; DynamicArray<ArticulatedRigidBody*> bodies; bodies.push_back(root); int currentBody = 0; while ((uint)currentBody<bodies.size()){ //add all the children joints to the list for (int i=0;i<bodies[currentBody]->getChildJointCount();i++){ Joint *j = bodies[currentBody]->getChildJoint(i); jointOrder.push_back( getJointIndex(j->getName()) ); bodies.push_back(bodies[currentBody]->getChildJoint(i)->getChild()); } currentBody++; } reverseJointOrder.resize( jointOrder.size() ); for( uint i=0; i < jointOrder.size(); ++i ) reverseJointOrder[jointOrder[i]] = i; }
Json::Value FileSaver::StoreSkeleton(const SkeletonData& skeleton) { Json::Value value; const std::map<ee::SprPtr, std::vector<Joint*> >& map_joints = skeleton.GetMapJoints(); std::map<ee::SprPtr, std::vector<Joint*> >::const_iterator itr = map_joints.begin(); for (int i = 0; itr != map_joints.end(); ++itr, ++i) { value[i]["sprite"] = itr->first->GetName(); for (int j = 0, m = itr->second.size(); j < m; ++j) { Joint* joint = itr->second[j]; Json::Value joint_val; joint_val["id"] = joint->GetID(); joint_val["rx"] = joint->GetCurrRelativePos().x; joint_val["ry"] = joint->GetCurrRelativePos().y; if (const Joint* parent = joint->GetParent()) { joint_val["parent"] = parent->GetID(); } const std::set<Joint*>& children = joint->GetChildren(); std::set<Joint*>::const_iterator itr_child = children.begin(); for (int k = 0; itr_child != children.end(); ++itr_child, ++k) { joint_val["children"][k] = (*itr_child)->GetID(); } value[i]["joints"][j] = joint_val; } } return value; }
//============================================================================== Eigen::Vector3d State::_getJointPosition(BodyNode* _bodyNode) const { Joint* parentJoint = _bodyNode->getParentJoint(); Eigen::Vector3d localJointPosition = parentJoint->getTransformFromChildBodyNode().translation(); return _bodyNode->getTransform() * localJointPosition; }
/** This method makes it possible to evaluate the debug pose at any phase angle */ void SimBiController::updateTrackingPose(DynamicArray<double>& trackingPose, double phiToUse){ if( phiToUse < 0 ) phiToUse = phi; if( phiToUse > 1 ) phiToUse = 1; trackingPose.clear(); this->character->getState(&trackingPose); ReducedCharacterState debugRS(&trackingPose); //always start from a neutral desired pose, and build from there... for (int i=0;i<jointCount;i++){ debugRS.setJointRelativeOrientation(Quaternion(1, 0, 0, 0), i); debugRS.setJointRelativeAngVelocity(Vector3d(), i); controlParams[i].relToCharFrame = false; } //and this is the desired orientation for the root Quaternion qRootD(1, 0, 0, 0); SimBiConState* curState = states[FSMStateIndex]; for (int i=0;i<curState->getTrajectoryCount();i++){ //now we have the desired rotation angle and axis, so we need to see which joint this is intended for int jIndex = curState->sTraj[i]->getJointIndex(stance); //if the index is -1, it must mean it's the root's trajectory. Otherwise we should give an error if (curState->sTraj[i]->relToCharFrame == true || jIndex == swingHipIndex) controlParams[jIndex].relToCharFrame = true; Vector3d d0, v0; computeD0( phiToUse, &d0 ); computeV0( phiToUse, &v0 ); Quaternion newOrientation = curState->sTraj[i]->evaluateTrajectory(this, character->getJoint(jIndex), stance, phiToUse, d - d0, v - v0); if (jIndex == -1){ qRootD = newOrientation; }else{ debugRS.setJointRelativeOrientation(newOrientation, jIndex); } } debugRS.setOrientation(qRootD); //now, we'll make one more pass, and make sure that the orientations that are relative to the character frame are drawn that way for (int i=0;i<jointCount;i++){ if (controlParams[i].relToCharFrame){ Quaternion temp; Joint* j = character->getJoint(i); ArticulatedRigidBody* parent = j->getParent(); while (parent != root){ j = parent->getParentJoint(); parent = j->getParent(); temp = debugRS.getJointRelativeOrientation(character->getJointIndex(j->name)) * temp; } temp = qRootD * temp; temp = temp.getComplexConjugate() * debugRS.getJointRelativeOrientation(i); debugRS.setJointRelativeOrientation(temp, i); } } }
void CollisionSpace::drawCollisionPoints() { for (unsigned int i = 0; i < m_Robot->getNumberOfJoints(); i++) { Joint* jnt = m_Robot->getJoint(i); Eigen::Transform3d T = jnt->getMatrixPos(); std::vector<CollisionPoint>& points = m_sampler->getCollisionPoints(jnt); for (unsigned int j = 0; j < points.size(); j++) { // if (points[j].getSegmentNumber() <ENV.getDouble(Env::extensionStep)) // continue; if (points[j].m_is_colliding) { double color[4]; color[1] = 0.0; // green color[2] = 0.0; // blue color[0] = 1.0; // red color[3] = 0.7; // transparency g3d_set_color(Any, color); } bool yellow = (!points[j].m_is_colliding); points[j].draw(T, yellow); } } }
double CollisionSpace::cost(const Configuration& q) const { // bool colliding = false; double distance = 0.0; double potential = 0.0; // colliding = // isRobotColliding( distance, potential ); double cost = 0.0; Eigen::Vector3d position, gradient; for (unsigned int i = 0; i < m_Robot->getNumberOfJoints(); i++) { Joint* jnt = m_Robot->getJoint(i); std::vector<CollisionPoint>& points = m_sampler->getCollisionPoints(jnt); if (points.empty()) continue; Eigen::Transform3d T = jnt->getMatrixPos(); for (unsigned int j = 0; j < points.size(); j++) { position = T * points[j].getPosition(); getCollisionPointPotentialGradient( points[j], position, distance, potential, gradient); if (potential < EPS6) { potential = EPS6; } cost += potential; } } return 10 * cost; }
void check_self_consistency(SkeletonPtr skeleton) { for(size_t i=0; i<skeleton->getNumBodyNodes(); ++i) { BodyNode* bn = skeleton->getBodyNode(i); EXPECT_TRUE(bn->getIndexInSkeleton() == i); EXPECT_TRUE(skeleton->getBodyNode(bn->getName()) == bn); Joint* joint = bn->getParentJoint(); EXPECT_TRUE(skeleton->getJoint(joint->getName()) == joint); for(size_t j=0; j<joint->getNumDofs(); ++j) { DegreeOfFreedom* dof = joint->getDof(j); EXPECT_TRUE(dof->getIndexInJoint() == j); EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof); } } for(size_t i=0; i<skeleton->getNumDofs(); ++i) { DegreeOfFreedom* dof = skeleton->getDof(i); EXPECT_TRUE(dof->getIndexInSkeleton() == i); EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof); } }
void Boxes::drawCollisionPoints() { for( size_t i=0; i<active_joints_.size(); i++ ) { Joint* jnt = robot_->getJoint( active_joints_[i] ); std::vector<CollisionPoint>& points = sampler_->getCollisionPoints(jnt); Eigen::Transform3d T = jnt->getMatrixPos(); cout << "joint : " << active_joints_[i] << " , points size : " << points.size() << endl; for( size_t j=0; j<points.size(); j++ ) { if( points[j].m_is_colliding ) { double color[4]; color[0] = 1.0; // (r)ed color[1] = 0.0; // (g)reen color[2] = 0.0; // (b)lue color[3] = 0.7; // transparency g3d_set_color(Any,color); } bool yellow = true; //bool yellow = (!points[j].m_is_colliding); points[j].draw( T, yellow ); } } }
int w_Joint_getReactionTorque(lua_State *L) { Joint *t = luax_checkjoint(L, 1); float inv_dt = (float)luaL_checknumber(L, 2); lua_pushnumber(L, t->getReactionTorque(inv_dt)); return 1; }
//todo плохо что надо учитывать interact void PhysLevelObject::requestDelete(bool instant) { for(auto& iter : destroyListeners) { iter(); } // Очень странно, ну а вдруг if(body==nullptr) { LevelObject::requestDelete(instant); return; } // Родителький requestDelete вызываться не будет, поэтому делаем за него удаление interact-а if(getInteract()) { getInteract()->getLayer()->deleteObject(getInteract()); } for(b2JointEdge* edge = getBody()->GetJointList(); edge; edge=edge->next) { Joint* j = static_cast<Joint*>(edge->joint->GetUserData()); j->getLayer()->deleteObject(j); } Game::getPhysicsSystem()->deleteObject(body); Game::getPhysicsSystem()->addPostStepAction([this]() { delete this; }); }
void Camera::processFrame() { this->capture.read(this->cameraImage); for (unsigned int i = 0 ; i < this->objectsToTrack.size() ; i++) { cvtColor(this->cameraImage, this->HSVImage, COLOR_BGR2HSV); inRange(this->HSVImage, this->objectsToTrack[i]->getHSVmin(), this->objectsToTrack[i]->getHSVmax(), this->thresholdImage); this->morphOps(this->thresholdImage); this->trackFilteredObject(this->objectsToTrack[i], this->thresholdImage, this->HSVImage, this->cameraImage); if (this->head != NULL && i == this->focusObjIndex) { Joint *pan = head->getJoint(PAN); Joint *tilt = head->getJoint(TILT); float x = this->objectsToTrack[i]->getXPos(); float y = this->objectsToTrack[i]->getYPos(); float half_width = float(CAMERA_WIDTH) / 2.0; float half_height = float(CAMERA_HEIGHT) / 2.0; float pan_angle = ((x - half_width) / half_width)*(CAMERA_HORIZONTAL_FIELD/2.0); float tilt_angle = ((y - half_height) / half_height)*(CAMERA_VERTICAL_FIELD/2.0); this->focusPan = pan->getAngle() - pan_angle; this->focusTilt = tilt->getAngle() - tilt_angle; } } imshow("Dimitri's Vision", cameraImage); }
void PoseIK::scalingJacobian( math::matrixN& J ) { unsigned int i, j; double c = 0.0f; unsigned int jj = 0; for( j=0; j < NUM_JOINTS_IN_HUMAN; j++ ) { Joint* joint = skeleton->getHumanJoint( j ); if( joint ) { unsigned int dof = joint->getDOF(); switch( dof ) { case 6 : c = PoseIK::RigidityCoeff[ j ]; for( i=0; i < num_equations; i++ ) { J[i][jj ] *= c; J[i][jj+1] *= c; J[i][jj+2] *= c; } c = PoseIK::RigidityCoeff[ j ]; for( i=0; i < num_equations; i++ ) { J[i][jj+3] *= c; J[i][jj+4] *= c; J[i][jj+5] *= c; } break; case 3 : c = PoseIK::RigidityCoeff[ j ]; for( i=0; i < num_equations; i++ ) { J[i][jj ] *= c; J[i][jj+1] *= c; J[i][jj+2] *= c; } break; case 1 : c = PoseIK::RigidityCoeff[ j ]; for( i=0; i < num_equations; i++ ) { J[i][jj ] *= c; } break; default : break; } jj += dof; } } }
void Animation::AnimateSkeleton(Skeleton* targetSkeleton, unsigned int frameA, unsigned int frameB, float interpolation) const { #if NAZARA_UTILITY_SAFE if (!m_impl) { NazaraError("Animation not created"); return; } if (m_impl->type != AnimationType_Skeletal) { NazaraError("Animation is not skeletal"); return; } if (!targetSkeleton || !targetSkeleton->IsValid()) { NazaraError("Target skeleton is invalid"); return; } if (targetSkeleton->GetJointCount() != m_impl->jointCount) { NazaraError("Target skeleton joint count must match animation joint count"); return; } if (frameA >= m_impl->frameCount) { NazaraError("Frame A is out of range (" + String::Number(frameA) + " >= " + String::Number(m_impl->frameCount) + ')'); return; } if (frameB >= m_impl->frameCount) { NazaraError("Frame B is out of range (" + String::Number(frameB) + " >= " + String::Number(m_impl->frameCount) + ')'); return; } #endif #ifdef NAZARA_DEBUG if (interpolation < 0.f || interpolation > 1.f) { NazaraError("Interpolation must be in range [0..1] (Got " + String::Number(interpolation) + ')'); return; } #endif for (unsigned int i = 0; i < m_impl->jointCount; ++i) { Joint* joint = targetSkeleton->GetJoint(i); SequenceJoint& sequenceJointA = m_impl->sequenceJoints[frameA*m_impl->jointCount + i]; SequenceJoint& sequenceJointB = m_impl->sequenceJoints[frameB*m_impl->jointCount + i]; joint->SetPosition(Vector3f::Lerp(sequenceJointA.position, sequenceJointB.position, interpolation)); joint->SetRotation(Quaternionf::Slerp(sequenceJointA.rotation, sequenceJointB.rotation, interpolation)); joint->SetScale(Vector3f::Lerp(sequenceJointA.scale, sequenceJointB.scale, interpolation)); } }
Control::Control(uint8_t id1, uint8_t id2, uint8_t id3) { jointTC.SetId(id1); jointCT.SetId(id2); jointFT.SetId(id3); }
void CollisionSpace::addEnvPoints() { // Add Static Obstacles for (int i = 0; i < XYZ_ENV->no; i++) { PointCloud& cloud = m_sampler->getPointCloud(XYZ_ENV->o[i]); for (unsigned int j = 0; j < cloud.size(); j++) m_points_to_add.push_back(cloud[j]); } // Add Moving Obstacles Scene* sc = global_Project->getActiveScene(); for (unsigned int i = 0; i < sc->getNumberOfRobots(); i++) { Robot* mov_obst = sc->getRobot(i); // The robot is not a robot or a human if (!((mov_obst->getName().find("ROBOT") != std::string::npos) || (mov_obst->getName().find("HUMAN") != std::string::npos) || mov_obst->getName() == "rob1" || mov_obst->getName() == "rob2" || mov_obst->getName() == "rob3" || mov_obst->getName() == "rob4")) { cout << "Adding : " << mov_obst->getName() << endl; Joint* jnt = mov_obst->getJoint(1); if (static_cast<p3d_jnt*>(jnt->getP3dJointStruct())->o == NULL) continue; PointCloud& cloud = m_sampler->getPointCloud( static_cast<p3d_jnt*>(jnt->getP3dJointStruct())->o); for (int j = 0; j < int(cloud.size()); j++) { m_points_to_add.push_back(jnt->getMatrixPos() * cloud[j]); } } } }
void NiutHumanReader::updateJoint(int i, int j, Joint& curJoint, int toasterId, std::vector<int>& trackedJoints, const niut_msgs::niut_HUMAN_LIST::ConstPtr& msg) { std::map<int, std::string> niutToString; niutToString[0] = "HEAD"; niutToString[3] = "TORSO"; niutToString[9] = "L_HAND"; niutToString[15] = "R_HIP"; niutToString[22] = "R_HAND"; double x, y, z; int niutJoint = trackedJoints[j]; std::string jointString = niutToString[niutJoint]; //Allocate Joint if needed if (lastConfig_[toasterId]->skeleton_[jointString] == NULL) { Joint* tmpJoint = new Joint(10001 + j, toasterId); tmpJoint->setName(jointString); tmpJoint->setAgentId(toasterId); lastConfig_[toasterId]->skeleton_[jointString] = tmpJoint; } x = msg->filtered_users[i].skeleton.joint[niutJoint].position.x; y = msg->filtered_users[i].skeleton.joint[niutJoint].position.y; z = msg->filtered_users[i].skeleton.joint[niutJoint].position.z; curJoint.position_.set<0>(x); curJoint.position_.set<1>(y); curJoint.position_.set<2>(z); projectJoint(curJoint, kinectPos_); }
void Joint::addChild(QTreeWidgetItem *child) { QTreeWidgetItem::addChild(child); Joint* j = (Joint*)child; children.push_back(j); j->setParent(this); }
Joint *luax_checkjoint(lua_State *L, int idx) { Joint *t = luax_checktype<Joint>(L, idx); if (!t->isValid()) luaL_error(L, "Attempt to use destroyed joint."); return t; }
int w_Joint_getType(lua_State *L) { Joint *t = luax_checkjoint(L, 1); const char *type = ""; Joint::getConstant(t->getType(), type); lua_pushstring(L, type); return 1; }
Joint MakeJoint(vector<RoadPoint> const & points) { Joint joint; for (auto const & point : points) joint.AddPoint(point); return joint; }
void Canvas::load(string path) { cout << "load" << endl; for (int i = 0; i < lines.size(); i++) { lines[i]->release(); delete lines[i]; } lines.clear(); while (!joints.empty()) { delete joints.back(); joints.pop_back(); } int lines_n; int n; ifstream file(path.c_str()); if (file.is_open()) { int i = 0; file >> n; for (int i = 0; i < n; i++) { Joint *v = new Joint(); v->setupGui(); file >> v->p.x; file >> v->p.y; int fixed; file >> fixed; v->fixed = (bool)fixed; *v->fixed_toggle = v->fixed; v->joint_type = JOINT_REVOLUTE; v->id = i; joints.push_back(v); } file >> lines_n; for (int i = 0; i < lines_n; i++) { lines.push_back(new Polyline()); Polyline *l = lines.back(); l->id = lines.size() - 1; file >> n; file >> l->closed; for (int i = 0; i < n; i++) { Vertex v; file >> v.p.x; file >> v.p.y; l->addBack(&v); } if (l->closed) { l->back->next = l->front; l->front->prev = l->back; } l->update(); } file.close(); }
Joint* PhysicsSim::createJoint(PhysicalObject* obj1, PhysicalObject* obj2, Vector3D anchor, Vector3D axis) { Joint* jt = new Joint(obj1, obj2, anchor, axis); dynamicsWorld->addConstraint(jt->getConstraint(), true); joints_list.push_back(jt); return jt; }
bool GameObjectManager::initializeCharacterNodeScale(Node* node) { Joint* joint = dynamic_cast<Joint*>(node); if (joint) { joint->setScale(CHARACTER_SCALE, CHARACTER_SCALE, CHARACTER_SCALE); } return true; }
static void CreatJoint( const AcDbObjectId& host, const AcGePoint3d& pt, double angle ) { Joint* pStation = new Joint( pt, angle ); // 关联到巷道上 pStation->setRelatedGE( host ); // 初始化并提交到数据库 if( !ArxUtilHelper::PostToModelSpace( pStation ) ) delete pStation; }
const Joint& Robot::const_joint(size_t jointIndex) const { if(jointIndex < nJoints()) return *joints_[jointIndex]; Joint* invalidJoint = new Joint; invalidJoint->name("invalid"); return *invalidJoint; }