C++ Joint类代码示例

/* 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;
    });
}