예제 #1
0
bool RobotConfig::setJointValues( RobotPtr r )
{
	if (!r)
		return false;
	WriteLockPtr lock = r->getWriteLock();

	std::map < std::string, float > jv = getRobotNodeJointValueMap();
	std::map< std::string, float >::const_iterator i = jv.begin();

	// first check if all nodes are present
	while (i != jv.end())
	{
		if (!r->hasRobotNode(i->first))
			return false;
		i++;
	}

	// apply jv
	i = jv.begin();
	while (i != jv.end())
	{
		RobotNodePtr rn = r->getRobotNode(i->first);
		if (!rn)
			return false;
		rn->setJointValueNoUpdate(i->second);
		i++;
	}
	r->applyJointValues();
	return true;
}
예제 #2
0
void RobotNodeSet::setJointValues(const Eigen::VectorXf &jointValues)
{
	THROW_VR_EXCEPTION_IF(jointValues.rows() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl);
	RobotPtr rob = robot.lock();
	VR_ASSERT(rob);
	WriteLockPtr lock = rob->getWriteLock();
	for (unsigned int i=0;i<robotNodes.size();i++)
	{
		robotNodes[i]->setJointValueNoUpdate(jointValues[i]);
	}	
	if (kinematicRoot)
		kinematicRoot->updatePose();
	else
		rob->applyJointValues();
}
예제 #3
0
bool RobotConfig::setJointValues()
{
	RobotPtr r = robot.lock();
	if (!r)
	{
		VR_WARNING << "Robot is already deleted, skipping update..." << endl;
		return false;
	}
	WriteLockPtr lock = r->getWriteLock();
	for (std::map< RobotNodePtr, float >::const_iterator i=configs.begin(); i!=configs.end(); i++ )
	{
		i->first->setJointValueNoUpdate(i->second);
	}
	r->applyJointValues();
	return true;
}
예제 #4
0
void RobotNodeSet::setJointValues( const RobotConfigPtr jointValues )
{
	VR_ASSERT(jointValues);
	RobotPtr rob = robot.lock();
	VR_ASSERT(rob);
	WriteLockPtr lock = rob->getWriteLock();
	
	for (unsigned int i=0;i<robotNodes.size();i++)
	{
		if (jointValues->hasConfig(robotNodes[i]->getName()))
			robotNodes[i]->setJointValueNoUpdate(jointValues->getConfig(robotNodes[i]->getName()));
	}
	if (kinematicRoot)
		kinematicRoot->updatePose();
	else
		rob->applyJointValues();

}