<commit_before>/***************************************************************************
 *   Copyright (C) 2006 by FThauer FHammer   *
 *   f.thauer@web.de   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "lobbychat.h"
#define IN_INCLUDE_LIBIRC_H
#include <core/libircclient/include/libirc_rfcnumeric.h>
#include <net/socket_msg.h>

#include "gamelobbydialogimpl.h"
#include "session.h"
#include "configfile.h"

using namespace std;

LobbyChat::LobbyChat(gameLobbyDialogImpl* l, ConfigFile *c) : myLobby(l), myConfig(c)
{

	chatInputCompleter = new QCompleter(myLobby->treeWidget_NickList->model());
	chatInputCompleter->setCaseSensitivity(Qt::CaseInsensitive);
	chatInputCompleter->setCompletionMode(QCompleter::InlineCompletion);
 	myLobby->lineEdit_ChatInput->setCompleter(chatInputCompleter);

}

LobbyChat::~LobbyChat()
{

}

void LobbyChat::sendMessage() {

	if (myNick.size())
	{
		fillChatLinesHistory(myLobby->lineEdit_ChatInput->text());
		QString tmpMsg(myLobby->lineEdit_ChatInput->text());
		if (tmpMsg.size())
		{
			myLobby->getSession().sendIrcChatMessage(tmpMsg.toUtf8().constData());
			myLobby->lineEdit_ChatInput->setText("");

			displayMessage(myNick, tmpMsg);
		}
	}
}

void LobbyChat::connected(QString server)
{
	myLobby->textBrowser_ChatDisplay->append(tr("Successfully connected to") + " " + server + ".");
}

void LobbyChat::selfJoined(QString ownName, QString channel)
{
	myNick = ownName;
	myLobby->textBrowser_ChatDisplay->append(tr("Joined channel:") + " " + channel + " " + tr("as user") + " " + ownName + ".");
	myLobby->textBrowser_ChatDisplay->append("");
	myLobby->lineEdit_ChatInput->setEnabled(true);
	myLobby->lineEdit_ChatInput->setFocus();
}

void LobbyChat::playerJoined(QString playerName)
{
	QTreeWidgetItem *item = new QTreeWidgetItem(myLobby->treeWidget_NickList, 0);
	item->setData(0, Qt::DisplayRole, playerName);

	myLobby->treeWidget_NickList->sortItems(0, Qt::AscendingOrder);
}

void LobbyChat::playerChanged(QString oldNick, QString newNick)
{
	QList<QTreeWidgetItem *> tmpList(myLobby->treeWidget_NickList->findItems(oldNick, Qt::MatchExactly));
	if (!tmpList.empty())
		tmpList.front()->setData(0, Qt::DisplayRole, newNick);
	else
	{
		tmpList = myLobby->treeWidget_NickList->findItems("@" + oldNick, Qt::MatchExactly);
		if (!tmpList.empty())
			tmpList.front()->setData(0, Qt::DisplayRole, "@" + newNick);
	}

	if (myNick == oldNick)
		myNick = newNick;
}

void LobbyChat::playerKicked(QString nickName, QString byWhom, QString reason)
{
	if (myNick == nickName)
	{
		myLobby->accept();
		QMessageBox::warning(myLobby, tr("Network Notification"),
			tr("You were kicked from the server."),
				QMessageBox::Close);
	}
	else
		myLobby->textBrowser_ChatDisplay->append(nickName + " " + tr("was kicked from the server by") + " " + byWhom + " (" + reason + ")");
}

void LobbyChat::playerLeft(QString playerName)
{
	QList<QTreeWidgetItem *> tmpList(myLobby->treeWidget_NickList->findItems(playerName, Qt::MatchExactly));
	if (!tmpList.empty())
		myLobby->treeWidget_NickList->takeTopLevelItem(myLobby->treeWidget_NickList->indexOfTopLevelItem(tmpList.front()));

	myLobby->treeWidget_NickList->sortItems(0, Qt::AscendingOrder);
}

void LobbyChat::displayMessage(QString playerName, QString message) {

	QString tempMsg;
	if(message.contains(QString::fromUtf8(myConfig->readConfigString("MyName").c_str()), Qt::CaseInsensitive)) {
		tempMsg = QString(""+message+"");
		if(myLobby->isVisible() && myConfig->readConfigInt("PlayLobbyChatNotification"))
			myLobby->getMyW()->getMySDLPlayer()->playSound("lobbychatnotify",0);
	}
	else {
		tempMsg = message;
	}
	myLobby->textBrowser_ChatDisplay->append(playerName + ": " + tempMsg);
}

void LobbyChat::checkInputLength(QString string) {

	 if(string.toUtf8().length() > 120) myLobby->lineEdit_ChatInput->setMaxLength(string.length());
}

void LobbyChat::clearChat() {

	myNick = "";
	myLobby->treeWidget_NickList->clear();
	myLobby->textBrowser_ChatDisplay->clear();
	myLobby->textBrowser_ChatDisplay->append(tr("Connecting to Chat server..."));
	myLobby->lineEdit_ChatInput->setEnabled(false);
/*	QStringList wordList;
	wordList << "alpha" << "omega" << "omicron" << "zeta";*/

// 	QCompleter *completer = new QCompleter(wordList, this);
// 	completer->setCaseSensitivity(Qt::CaseInsensitive);
// 	completer->setCompletionMode(QCompleter::InlineCompletion);
// // 	lineEdit_ChatInput->setCompleter(completer);

}

void LobbyChat::chatError(int errorCode)
{
	QString errorMsg;
	switch (errorCode)
	{
		case ERR_IRC_INTERNAL :
		case ERR_IRC_SELECT_FAILED :
		case ERR_IRC_RECV_FAILED :
		case ERR_IRC_SEND_FAILED :
		case ERR_IRC_INVALID_PARAM :
			errorMsg = tr("An internal IRC error has occured:") + " " + QString::number(errorCode);
			break;
		case ERR_IRC_CONNECT_FAILED :
			errorMsg = tr("Could not connect to the IRC server. Chat will be unavailable.");
			break;
		case ERR_IRC_TERMINATED :
			errorMsg = tr("The IRC server terminated the connection. Chat will be unavailable.");
			break;
		case ERR_IRC_TIMEOUT :
			errorMsg = tr("An IRC action timed out.");
			break;
	}
	if (errorMsg.size())
		myLobby->textBrowser_ChatDisplay->append(errorMsg);
}

void LobbyChat::chatServerError(int errorCode)
{
	QString errorMsg;
	switch (errorCode)
	{
		case LIBIRC_RFC_ERR_NOSUCHNICK :
		case LIBIRC_RFC_ERR_NOSUCHCHANNEL :
		case LIBIRC_RFC_ERR_CANNOTSENDTOCHAN :
		case LIBIRC_RFC_ERR_TOOMANYCHANNELS :
		case LIBIRC_RFC_ERR_WASNOSUCHNICK :
		case LIBIRC_RFC_ERR_TOOMANYTARGETS :
		case LIBIRC_RFC_ERR_NOSUCHSERVICE :
		case LIBIRC_RFC_ERR_NOORIGIN :
		case LIBIRC_RFC_ERR_NORECIPIENT :
		case LIBIRC_RFC_ERR_NOTEXTTOSEND :
		case LIBIRC_RFC_ERR_NOTOPLEVEL :
		case LIBIRC_RFC_ERR_WILDTOPLEVEL :
		case LIBIRC_RFC_ERR_BADMASK :
		case LIBIRC_RFC_ERR_UNKNOWNCOMMAND :
		case LIBIRC_RFC_ERR_NOMOTD :
		case LIBIRC_RFC_ERR_NOADMININFO :
		case LIBIRC_RFC_ERR_FILEERROR :
		case LIBIRC_RFC_ERR_NONICKNAMEGIVEN :
		case LIBIRC_RFC_ERR_ERRONEUSNICKNAME :
		case LIBIRC_RFC_ERR_UNAVAILRESOURCE :
		case LIBIRC_RFC_ERR_USERNOTINCHANNEL :
		case LIBIRC_RFC_ERR_NOTONCHANNEL :
		case LIBIRC_RFC_ERR_USERONCHANNEL :
		case LIBIRC_RFC_ERR_NOLOGIN :
		case LIBIRC_RFC_ERR_SUMMONDISABLED :
		case LIBIRC_RFC_ERR_USERSDISABLED :
		case LIBIRC_RFC_ERR_NOTREGISTERED :
		case LIBIRC_RFC_ERR_NEEDMOREPARAMS :
		case LIBIRC_RFC_ERR_ALREADYREGISTRED :
		case LIBIRC_RFC_ERR_NOPERMFORHOST :
		case LIBIRC_RFC_ERR_PASSWDMISMATCH :
		case LIBIRC_RFC_ERR_YOUREBANNEDCREEP :
		case LIBIRC_RFC_ERR_YOUWILLBEBANNED :
		case LIBIRC_RFC_ERR_KEYSET :
		case LIBIRC_RFC_ERR_CHANNELISFULL :
		case LIBIRC_RFC_ERR_UNKNOWNMODE :
		case LIBIRC_RFC_ERR_INVITEONLYCHAN :
		case LIBIRC_RFC_ERR_BANNEDFROMCHAN :
		case LIBIRC_RFC_ERR_BADCHANNELKEY :
		case LIBIRC_RFC_ERR_BADCHANMASK :
		case LIBIRC_RFC_ERR_NOCHANMODES :
		case LIBIRC_RFC_ERR_BANLISTFULL :
		case LIBIRC_RFC_ERR_NOPRIVILEGES :
		case LIBIRC_RFC_ERR_CHANOPRIVSNEEDED :
		case LIBIRC_RFC_ERR_CANTKILLSERVER :
		case LIBIRC_RFC_ERR_RESTRICTED :
		case LIBIRC_RFC_ERR_UNIQOPPRIVSNEEDED :
		case LIBIRC_RFC_ERR_NOOPERHOST :
		case LIBIRC_RFC_ERR_UMODEUNKNOWNFLAG :
		case LIBIRC_RFC_ERR_USERSDONTMATCH :
			errorMsg = tr("The IRC server reported an error:") + " " + QString::number(errorCode);
			break;
	}
//	if (errorMsg.size())
//		myLobby->textBrowser_ChatDisplay->append(errorMsg);
}

void LobbyChat::fillChatLinesHistory(QString fillString) {

	chatLinesHistory << fillString;
	if(chatLinesHistory.size() > 50) chatLinesHistory.removeFirst();

// 	QStringList::const_iterator constIterator;
//      	for (constIterator = chatLinesHistory.constBegin(); constIterator != chatLinesHistory.constEnd(); ++constIterator) {
//          	cout << (*constIterator).toLocal8Bit().constData() << endl;
// 	}
}

void LobbyChat::showChatHistoryIndex(int index) {

	if(index <= chatLinesHistory.size() && index > 0) {

// 		cout << chatLinesHistory.size() << " : " <<  index << endl;
		myLobby->lineEdit_ChatInput->setText(chatLinesHistory.at(chatLinesHistory.size()-(index)));
	}
}
<commit_msg>nick-autocompletition enhancement<commit_after>/***************************************************************************
 *   Copyright (C) 2006 by FThauer FHammer   *
 *   f.thauer@web.de   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "lobbychat.h"
#define IN_INCLUDE_LIBIRC_H
#include <core/libircclient/include/libirc_rfcnumeric.h>
#include <net/socket_msg.h>

#include "gamelobbydialogimpl.h"
#include "session.h"
#include "configfile.h"

using namespace std;

LobbyChat::LobbyChat(gameLobbyDialogImpl* l, ConfigFile *c) : myLobby(l), myConfig(c)
{

	chatInputCompleter = new QCompleter(myLobby->treeWidget_NickList->model());
	chatInputCompleter->setCaseSensitivity(Qt::CaseInsensitive);
	chatInputCompleter->setCompletionMode(QCompleter::PopupCompletion);
 	myLobby->lineEdit_ChatInput->setCompleter(chatInputCompleter);

}

LobbyChat::~LobbyChat()
{

}

void LobbyChat::sendMessage() {

	if (myNick.size())
	{
		fillChatLinesHistory(myLobby->lineEdit_ChatInput->text());
		QString tmpMsg(myLobby->lineEdit_ChatInput->text());
		if (tmpMsg.size())
		{
			myLobby->getSession().sendIrcChatMessage(tmpMsg.toUtf8().constData());
			myLobby->lineEdit_ChatInput->setText("");

			displayMessage(myNick, tmpMsg);
		}
	}
}

void LobbyChat::connected(QString server)
{
	myLobby->textBrowser_ChatDisplay->append(tr("Successfully connected to") + " " + server + ".");
}

void LobbyChat::selfJoined(QString ownName, QString channel)
{
	myNick = ownName;
	myLobby->textBrowser_ChatDisplay->append(tr("Joined channel:") + " " + channel + " " + tr("as user") + " " + ownName + ".");
	myLobby->textBrowser_ChatDisplay->append("");
	myLobby->lineEdit_ChatInput->setEnabled(true);
	myLobby->lineEdit_ChatInput->setFocus();
}

void LobbyChat::playerJoined(QString playerName)
{
	QTreeWidgetItem *item = new QTreeWidgetItem(myLobby->treeWidget_NickList, 0);
	item->setData(0, Qt::DisplayRole, playerName);

	myLobby->treeWidget_NickList->sortItems(0, Qt::AscendingOrder);
}

void LobbyChat::playerChanged(QString oldNick, QString newNick)
{
	QList<QTreeWidgetItem *> tmpList(myLobby->treeWidget_NickList->findItems(oldNick, Qt::MatchExactly));
	if (!tmpList.empty())
		tmpList.front()->setData(0, Qt::DisplayRole, newNick);
	else
	{
		tmpList = myLobby->treeWidget_NickList->findItems("@" + oldNick, Qt::MatchExactly);
		if (!tmpList.empty())
			tmpList.front()->setData(0, Qt::DisplayRole, "@" + newNick);
	}

	if (myNick == oldNick)
		myNick = newNick;
}

void LobbyChat::playerKicked(QString nickName, QString byWhom, QString reason)
{
	if (myNick == nickName)
	{
		myLobby->accept();
		QMessageBox::warning(myLobby, tr("Network Notification"),
			tr("You were kicked from the server."),
				QMessageBox::Close);
	}
	else
		myLobby->textBrowser_ChatDisplay->append(nickName + " " + tr("was kicked from the server by") + " " + byWhom + " (" + reason + ")");
}

void LobbyChat::playerLeft(QString playerName)
{
	QList<QTreeWidgetItem *> tmpList(myLobby->treeWidget_NickList->findItems(playerName, Qt::MatchExactly));
	if (!tmpList.empty())
		myLobby->treeWidget_NickList->takeTopLevelItem(myLobby->treeWidget_NickList->indexOfTopLevelItem(tmpList.front()));

	myLobby->treeWidget_NickList->sortItems(0, Qt::AscendingOrder);
}

void LobbyChat::displayMessage(QString playerName, QString message) {

	QString tempMsg;
	if(message.contains(QString::fromUtf8(myConfig->readConfigString("MyName").c_str()), Qt::CaseInsensitive)) {
		tempMsg = QString(""+message+"");
		if(myLobby->isVisible() && myConfig->readConfigInt("PlayLobbyChatNotification"))
			myLobby->getMyW()->getMySDLPlayer()->playSound("lobbychatnotify",0);
	}
	else {
		tempMsg = message;
	}
	myLobby->textBrowser_ChatDisplay->append(playerName + ": " + tempMsg);
}

void LobbyChat::checkInputLength(QString string) {

	 if(string.toUtf8().length() > 120) myLobby->lineEdit_ChatInput->setMaxLength(string.length());
}

void LobbyChat::clearChat() {

	myNick = "";
	myLobby->treeWidget_NickList->clear();
	myLobby->textBrowser_ChatDisplay->clear();
	myLobby->textBrowser_ChatDisplay->append(tr("Connecting to Chat server..."));
	myLobby->lineEdit_ChatInput->setEnabled(false);
/*	QStringList wordList;
	wordList << "alpha" << "omega" << "omicron" << "zeta";*/

// 	QCompleter *completer = new QCompleter(wordList, this);
// 	completer->setCaseSensitivity(Qt::CaseInsensitive);
// 	completer->setCompletionMode(QCompleter::InlineCompletion);
// // 	lineEdit_ChatInput->setCompleter(completer);

}

void LobbyChat::chatError(int errorCode)
{
	QString errorMsg;
	switch (errorCode)
	{
		case ERR_IRC_INTERNAL :
		case ERR_IRC_SELECT_FAILED :
		case ERR_IRC_RECV_FAILED :
		case ERR_IRC_SEND_FAILED :
		case ERR_IRC_INVALID_PARAM :
			errorMsg = tr("An internal IRC error has occured:") + " " + QString::number(errorCode);
			break;
		case ERR_IRC_CONNECT_FAILED :
			errorMsg = tr("Could not connect to the IRC server. Chat will be unavailable.");
			break;
		case ERR_IRC_TERMINATED :
			errorMsg = tr("The IRC server terminated the connection. Chat will be unavailable.");
			break;
		case ERR_IRC_TIMEOUT :
			errorMsg = tr("An IRC action timed out.");
			break;
	}
	if (errorMsg.size())
		myLobby->textBrowser_ChatDisplay->append(errorMsg);
}

void LobbyChat::chatServerError(int errorCode)
{
	QString errorMsg;
	switch (errorCode)
	{
		case LIBIRC_RFC_ERR_NOSUCHNICK :
		case LIBIRC_RFC_ERR_NOSUCHCHANNEL :
		case LIBIRC_RFC_ERR_CANNOTSENDTOCHAN :
		case LIBIRC_RFC_ERR_TOOMANYCHANNELS :
		case LIBIRC_RFC_ERR_WASNOSUCHNICK :
		case LIBIRC_RFC_ERR_TOOMANYTARGETS :
		case LIBIRC_RFC_ERR_NOSUCHSERVICE :
		case LIBIRC_RFC_ERR_NOORIGIN :
		case LIBIRC_RFC_ERR_NORECIPIENT :
		case LIBIRC_RFC_ERR_NOTEXTTOSEND :
		case LIBIRC_RFC_ERR_NOTOPLEVEL :
		case LIBIRC_RFC_ERR_WILDTOPLEVEL :
		case LIBIRC_RFC_ERR_BADMASK :
		case LIBIRC_RFC_ERR_UNKNOWNCOMMAND :
		case LIBIRC_RFC_ERR_NOMOTD :
		case LIBIRC_RFC_ERR_NOADMININFO :
		case LIBIRC_RFC_ERR_FILEERROR :
		case LIBIRC_RFC_ERR_NONICKNAMEGIVEN :
		case LIBIRC_RFC_ERR_ERRONEUSNICKNAME :
		case LIBIRC_RFC_ERR_UNAVAILRESOURCE :
		case LIBIRC_RFC_ERR_USERNOTINCHANNEL :
		case LIBIRC_RFC_ERR_NOTONCHANNEL :
		case LIBIRC_RFC_ERR_USERONCHANNEL :
		case LIBIRC_RFC_ERR_NOLOGIN :
		case LIBIRC_RFC_ERR_SUMMONDISABLED :
		case LIBIRC_RFC_ERR_USERSDISABLED :
		case LIBIRC_RFC_ERR_NOTREGISTERED :
		case LIBIRC_RFC_ERR_NEEDMOREPARAMS :
		case LIBIRC_RFC_ERR_ALREADYREGISTRED :
		case LIBIRC_RFC_ERR_NOPERMFORHOST :
		case LIBIRC_RFC_ERR_PASSWDMISMATCH :
		case LIBIRC_RFC_ERR_YOUREBANNEDCREEP :
		case LIBIRC_RFC_ERR_YOUWILLBEBANNED :
		case LIBIRC_RFC_ERR_KEYSET :
		case LIBIRC_RFC_ERR_CHANNELISFULL :
		case LIBIRC_RFC_ERR_UNKNOWNMODE :
		case LIBIRC_RFC_ERR_INVITEONLYCHAN :
		case LIBIRC_RFC_ERR_BANNEDFROMCHAN :
		case LIBIRC_RFC_ERR_BADCHANNELKEY :
		case LIBIRC_RFC_ERR_BADCHANMASK :
		case LIBIRC_RFC_ERR_NOCHANMODES :
		case LIBIRC_RFC_ERR_BANLISTFULL :
		case LIBIRC_RFC_ERR_NOPRIVILEGES :
		case LIBIRC_RFC_ERR_CHANOPRIVSNEEDED :
		case LIBIRC_RFC_ERR_CANTKILLSERVER :
		case LIBIRC_RFC_ERR_RESTRICTED :
		case LIBIRC_RFC_ERR_UNIQOPPRIVSNEEDED :
		case LIBIRC_RFC_ERR_NOOPERHOST :
		case LIBIRC_RFC_ERR_UMODEUNKNOWNFLAG :
		case LIBIRC_RFC_ERR_USERSDONTMATCH :
			errorMsg = tr("The IRC server reported an error:") + " " + QString::number(errorCode);
			break;
	}
//	if (errorMsg.size())
//		myLobby->textBrowser_ChatDisplay->append(errorMsg);
}

void LobbyChat::fillChatLinesHistory(QString fillString) {

	chatLinesHistory << fillString;
	if(chatLinesHistory.size() > 50) chatLinesHistory.removeFirst();

// 	QStringList::const_iterator constIterator;
//      	for (constIterator = chatLinesHistory.constBegin(); constIterator != chatLinesHistory.constEnd(); ++constIterator) {
//          	cout << (*constIterator).toLocal8Bit().constData() << endl;
// 	}
}

void LobbyChat::showChatHistoryIndex(int index) {

	if(index <= chatLinesHistory.size() && index > 0) {

// 		cout << chatLinesHistory.size() << " : " <<  index << endl;
		myLobby->lineEdit_ChatInput->setText(chatLinesHistory.at(chatLinesHistory.size()-(index)));
	}
}
<|endoftext|>


<commit_before>#include "iCubProprioception/iKinCADSuperimposer.h"

#include <exception>
#include <utility>

#include <iCub/ctrl/math.h>
#include <yarp/math/Math.h>
#include <yarp/os/LogStream.h>

using namespace yarp::dev;
using namespace yarp::math;
using namespace yarp::os;
using namespace yarp::sig;
using namespace iCub::ctrl;
using namespace iCub::iKin;

iKinCADSuperimposer::iKinCADSuperimposer(const ConstString& project_name, const ConstString& robot, const ConstString& camera,
                                         const SuperImpose::ObjFileMap& cad_hand, const ConstString& shader_path) :
    CADSuperimposer(project_name + "/iKinCADSuperimposer", robot, camera, cad_hand, shader_path)
{
    yInfo() << log_ID_ << "Invoked iKinCADSuperimposer (derived class) ctor...";

    /* Get head interfaces */
    if (!setHeadRemoteControlboard())
    {
        yError() << log_ID_ << "Head remote_controlboard errored!";
        throw std::runtime_error("Head remote_controlboard errored!");
    }

    /* Get torso interfaces */
    if (!setTorsoRemoteControlboard())
    {
        yError() << log_ID_ << "Torso remote_controlboard errored!";
        throw std::runtime_error("Torso remote_controlboard errored!");
    }

    /* Get arm interfaces */
    if (!setArmRemoteControlboard())
    {
        yError() << log_ID_ << "Arm remote_controlboard errored!";
        throw std::runtime_error("Arm remote_controlboard errored!");
    }

    /* Get arm Cartesian interfaces */
    if (!setArmCartesianController())
    {
        yError() << log_ID_ << "Arm cartesiancontrollerclient errored!";
        throw std::runtime_error("Arm cartesiancontrollerclient errored!");
    }

    /* Initialize right arm interface */
    yInfo() << log_ID_ << "Setting right arm interface.";

    right_arm_ = iCubArm("right");

    right_arm_.setAllConstraints(false);
    right_arm_.releaseLink(0);
    right_arm_.releaseLink(1);
    right_arm_.releaseLink(2);

    yInfo() << log_ID_ << "...iKinCADSuperimposer ctor completed!";
}

iKinCADSuperimposer::~iKinCADSuperimposer() noexcept { }

Vector iKinCADSuperimposer::getEndEffectorPose()
{
    Vector ee_x(3);
    Vector ee_o(4);

    itf_right_arm_cart_->getPose(ee_x, ee_o);

    Vector ee_pose(7);

    ee_pose.setSubvector(0, ee_x);
    ee_pose.setSubvector(3, ee_o);

    return ee_pose;
}

Vector iKinCADSuperimposer::getHeadEncoders()
{
    int enc_num;
    itf_head_encoders_->getAxes(&enc_num);

    Vector enc_head(enc_num);

    itf_head_encoders_->getEncoders(enc_head.data());

    return enc_head;
}

Vector iKinCADSuperimposer::getRightArmEncoders()
{
    int enc_num;
    itf_right_arm_encoders_->getAxes(&enc_num);

    Vector enc_arm(enc_num);

    itf_right_arm_encoders_->getEncoders(enc_arm.data());

    return enc_arm;
}

#if ICP_USE_ANALOGS == 1
Vector iKinCADSuperimposer::getRightHandAnalogs()
{
    int enc_num;
    itf_right_hand_analog_->getAxes(&enc_num);

    Vector enc_arm(enc_num);

    itf_right_hand_analog_->getEncoders(enc_arm.data());

    return enc_arm;
}
#endif

Vector iKinCADSuperimposer::getTorsoEncoders()
{
    int enc_num;
    itf_torso_encoders_->getAxes(&enc_num);

    Vector enc_torso(enc_num);

    itf_torso_encoders_->getEncoders(enc_torso.data());

    std::swap(enc_torso(0), enc_torso(2));

    return enc_torso;
}

void iKinCADSuperimposer::getExtraObjPoseMap(SuperImpose::ObjPoseMap& hand_pose)
{
    SuperImpose::ObjPose pose;

    Matrix H_forearm = right_arm_.getH(7, true);

    Vector j_x = H_forearm.getCol(3).subVector(0, 2);
    Vector j_o = dcm2axis(H_forearm);

    pose.assign(j_x.data(), j_x.data()+3);
    pose.insert(pose.end(), j_o.data(), j_o.data()+4);

    hand_pose.emplace("forearm", pose);
}

bool iKinCADSuperimposer::setHeadRemoteControlboard()
{
    Property head_remote_options;
    head_remote_options.put("device", "remote_controlboard");
    head_remote_options.put("local", "/" + ID_ + "/control_head");
    head_remote_options.put("remote", "/" + robot_ + "/head");

    drv_head_remote_.open(head_remote_options);
    if (drv_head_remote_.isValid())
    {
        yInfo() << log_ID_ << "Head remote_controlboard succefully opened.";

        drv_head_remote_.view(itf_head_encoders_);
        if (!itf_head_encoders_)
        {
            yError() << log_ID_ << "Error getting Head IEncoders interface in thread.";
            throw std::runtime_error("Error getting Head IEncoders interface in thread.");
        }

        return true;
    }
    else
    {
        yError() << log_ID_ << "Error opening Head remote_controlboard device.";
        return false;
    }
}

bool iKinCADSuperimposer::setTorsoRemoteControlboard()
{
    Property torso_remote_options;
    torso_remote_options.put("device", "remote_controlboard");
    torso_remote_options.put("local", "/" + ID_ + "/control_torso");
    torso_remote_options.put("remote", "/" + robot_ + "/torso");

    drv_torso_remote_.open(torso_remote_options);
    if (drv_torso_remote_.isValid())
    {
        yInfo() << log_ID_ << "Torso remote_controlboard succefully opened.";

        drv_torso_remote_.view(itf_torso_encoders_);
        if (!itf_torso_encoders_)
        {
            yError() << log_ID_ << "Error getting torso IEncoders interface in thread.";
            throw std::runtime_error("Error getting torso IEncoders interface in thread.");
        }

        return true;
    }
    else
    {
        yError() << log_ID_ << "Error opening Torso remote_controlboard device.";
        return false;
    }
}

bool iKinCADSuperimposer::setArmRemoteControlboard()
{
    Property rightarm_remote_options;
    rightarm_remote_options.put("device", "remote_controlboard");
    rightarm_remote_options.put("local", "/" + ID_ + "/control_right_arm");
    rightarm_remote_options.put("remote", "/" + robot_ + "/right_arm");

    drv_right_arm_remote_.open(rightarm_remote_options);
    if (drv_right_arm_remote_.isValid())
    {
        yInfo() << log_ID_ << "Right arm remote_controlboard succefully opened.";

        drv_right_arm_remote_.view(itf_right_arm_encoders_);
        if (!itf_right_arm_encoders_)
        {
            yError() << log_ID_ << "Error getting right arm IEncoders interface.";
            return false;
        }
    }
    else
    {
        yError() << log_ID_ << "Error opening right arm remote_controlboard device.";
        return false;
    }

#if ICP_USE_ANALOGS == 1
    Property righthand_remote_analog;
    righthand_remote_analog.put("device", "analogsensorclient");
    righthand_remote_analog.put("local",  "/" + ID_ + "/right_hand");
    righthand_remote_analog.put("remote", "/" + robot_ + "/right_hand/analog:o");

    drv_right_hand_analog_.open(righthand_remote_analog);
    if (drv_right_hand_analog_.isValid())
    {
        yInfo() << log_ID_ << "Right arm analogsensorclient succefully opened.";

        if (!drv_right_hand_analog_.view(itf_right_hand_analog_))
        {
            yError() << log_ID_ << "Error getting right hand IAnalogSensor interface!";
            throw std::runtime_error("Error getting right hand IAnalogSensor interface!");
        }
    }
    else
    {
        yError() << log_ID_ << "Error opening right arm analogsensorclient device.";
        return false;
    }
#endif

    return true;
}

bool iKinCADSuperimposer::setArmCartesianController()
{
    Property rightarm_cartesian_options;
    rightarm_cartesian_options.put("device", "cartesiancontrollerclient");
    rightarm_cartesian_options.put("local", "/" + ID_ + "/cart_right_arm");
    rightarm_cartesian_options.put("remote", "/" + robot_ + "/cartesianController/right_arm");

    drv_right_arm_cartesian_.open(rightarm_cartesian_options);
    if (drv_right_arm_cartesian_.isValid())
    {
        drv_right_arm_cartesian_.view(itf_right_arm_cart_);
        if (!itf_right_arm_cart_)
        {
            yError() << log_ID_ << "Error getting ICartesianControl interface.";
            return false;
        }
        yInfo() << log_ID_ << "cartesiancontrollerclient succefully opened.";
    }
    else
    {
        yError() << log_ID_ << "Error opening cartesiancontrollerclient device.";
        return false;
    }

    return true;
}

Vector iKinCADSuperimposer::readRootToEE()
{
    Vector enc_arm = getRightArmEncoders();

    Vector root_ee_enc(10);

    root_ee_enc.setSubvector(0, getTorsoEncoders());

    for (size_t i = 0; i < 7; ++i)
        root_ee_enc(i+3) = enc_arm(i);

    return root_ee_enc;
}
<commit_msg>Add missing setAng() call in iKinCADSuperimposer<commit_after>#include "iCubProprioception/iKinCADSuperimposer.h"

#include <exception>
#include <utility>

#include <iCub/ctrl/math.h>
#include <yarp/math/Math.h>
#include <yarp/os/LogStream.h>

using namespace yarp::dev;
using namespace yarp::math;
using namespace yarp::os;
using namespace yarp::sig;
using namespace iCub::ctrl;
using namespace iCub::iKin;

iKinCADSuperimposer::iKinCADSuperimposer(const ConstString& project_name, const ConstString& robot, const ConstString& camera,
                                         const SuperImpose::ObjFileMap& cad_hand, const ConstString& shader_path) :
    CADSuperimposer(project_name + "/iKinCADSuperimposer", robot, camera, cad_hand, shader_path)
{
    yInfo() << log_ID_ << "Invoked iKinCADSuperimposer (derived class) ctor...";

    /* Get head interfaces */
    if (!setHeadRemoteControlboard())
    {
        yError() << log_ID_ << "Head remote_controlboard errored!";
        throw std::runtime_error("Head remote_controlboard errored!");
    }

    /* Get torso interfaces */
    if (!setTorsoRemoteControlboard())
    {
        yError() << log_ID_ << "Torso remote_controlboard errored!";
        throw std::runtime_error("Torso remote_controlboard errored!");
    }

    /* Get arm interfaces */
    if (!setArmRemoteControlboard())
    {
        yError() << log_ID_ << "Arm remote_controlboard errored!";
        throw std::runtime_error("Arm remote_controlboard errored!");
    }

    /* Get arm Cartesian interfaces */
    if (!setArmCartesianController())
    {
        yError() << log_ID_ << "Arm cartesiancontrollerclient errored!";
        throw std::runtime_error("Arm cartesiancontrollerclient errored!");
    }

    /* Initialize right arm interface */
    yInfo() << log_ID_ << "Setting right arm interface.";

    right_arm_ = iCubArm("right");

    right_arm_.setAllConstraints(false);
    right_arm_.releaseLink(0);
    right_arm_.releaseLink(1);
    right_arm_.releaseLink(2);

    yInfo() << log_ID_ << "...iKinCADSuperimposer ctor completed!";
}

iKinCADSuperimposer::~iKinCADSuperimposer() noexcept { }

Vector iKinCADSuperimposer::getEndEffectorPose()
{
    Vector ee_x(3);
    Vector ee_o(4);

    itf_right_arm_cart_->getPose(ee_x, ee_o);

    Vector ee_pose(7);

    ee_pose.setSubvector(0, ee_x);
    ee_pose.setSubvector(3, ee_o);

    return ee_pose;
}

Vector iKinCADSuperimposer::getHeadEncoders()
{
    int enc_num;
    itf_head_encoders_->getAxes(&enc_num);

    Vector enc_head(enc_num);

    itf_head_encoders_->getEncoders(enc_head.data());

    return enc_head;
}

Vector iKinCADSuperimposer::getRightArmEncoders()
{
    int enc_num;
    itf_right_arm_encoders_->getAxes(&enc_num);

    Vector enc_arm(enc_num);

    itf_right_arm_encoders_->getEncoders(enc_arm.data());

    return enc_arm;
}

#if ICP_USE_ANALOGS == 1
Vector iKinCADSuperimposer::getRightHandAnalogs()
{
    int enc_num;
    itf_right_hand_analog_->getAxes(&enc_num);

    Vector enc_arm(enc_num);

    itf_right_hand_analog_->getEncoders(enc_arm.data());

    return enc_arm;
}
#endif

Vector iKinCADSuperimposer::getTorsoEncoders()
{
    int enc_num;
    itf_torso_encoders_->getAxes(&enc_num);

    Vector enc_torso(enc_num);

    itf_torso_encoders_->getEncoders(enc_torso.data());

    std::swap(enc_torso(0), enc_torso(2));

    return enc_torso;
}

void iKinCADSuperimposer::getExtraObjPoseMap(SuperImpose::ObjPoseMap& hand_pose)
{
    SuperImpose::ObjPose pose;

    right_arm_.setAng(CTRL_DEG2RAD * readRootToEE());

    Matrix H_forearm = right_arm_.getH(7, true);

    Vector j_x = H_forearm.getCol(3).subVector(0, 2);
    Vector j_o = dcm2axis(H_forearm);

    pose.assign(j_x.data(), j_x.data()+3);
    pose.insert(pose.end(), j_o.data(), j_o.data()+4);

    hand_pose.emplace("forearm", pose);
}

bool iKinCADSuperimposer::setHeadRemoteControlboard()
{
    Property head_remote_options;
    head_remote_options.put("device", "remote_controlboard");
    head_remote_options.put("local", "/" + ID_ + "/control_head");
    head_remote_options.put("remote", "/" + robot_ + "/head");

    drv_head_remote_.open(head_remote_options);
    if (drv_head_remote_.isValid())
    {
        yInfo() << log_ID_ << "Head remote_controlboard succefully opened.";

        drv_head_remote_.view(itf_head_encoders_);
        if (!itf_head_encoders_)
        {
            yError() << log_ID_ << "Error getting Head IEncoders interface in thread.";
            throw std::runtime_error("Error getting Head IEncoders interface in thread.");
        }

        return true;
    }
    else
    {
        yError() << log_ID_ << "Error opening Head remote_controlboard device.";
        return false;
    }
}

bool iKinCADSuperimposer::setTorsoRemoteControlboard()
{
    Property torso_remote_options;
    torso_remote_options.put("device", "remote_controlboard");
    torso_remote_options.put("local", "/" + ID_ + "/control_torso");
    torso_remote_options.put("remote", "/" + robot_ + "/torso");

    drv_torso_remote_.open(torso_remote_options);
    if (drv_torso_remote_.isValid())
    {
        yInfo() << log_ID_ << "Torso remote_controlboard succefully opened.";

        drv_torso_remote_.view(itf_torso_encoders_);
        if (!itf_torso_encoders_)
        {
            yError() << log_ID_ << "Error getting torso IEncoders interface in thread.";
            throw std::runtime_error("Error getting torso IEncoders interface in thread.");
        }

        return true;
    }
    else
    {
        yError() << log_ID_ << "Error opening Torso remote_controlboard device.";
        return false;
    }
}

bool iKinCADSuperimposer::setArmRemoteControlboard()
{
    Property rightarm_remote_options;
    rightarm_remote_options.put("device", "remote_controlboard");
    rightarm_remote_options.put("local", "/" + ID_ + "/control_right_arm");
    rightarm_remote_options.put("remote", "/" + robot_ + "/right_arm");

    drv_right_arm_remote_.open(rightarm_remote_options);
    if (drv_right_arm_remote_.isValid())
    {
        yInfo() << log_ID_ << "Right arm remote_controlboard succefully opened.";

        drv_right_arm_remote_.view(itf_right_arm_encoders_);
        if (!itf_right_arm_encoders_)
        {
            yError() << log_ID_ << "Error getting right arm IEncoders interface.";
            return false;
        }
    }
    else
    {
        yError() << log_ID_ << "Error opening right arm remote_controlboard device.";
        return false;
    }

#if ICP_USE_ANALOGS == 1
    Property righthand_remote_analog;
    righthand_remote_analog.put("device", "analogsensorclient");
    righthand_remote_analog.put("local",  "/" + ID_ + "/right_hand");
    righthand_remote_analog.put("remote", "/" + robot_ + "/right_hand/analog:o");

    drv_right_hand_analog_.open(righthand_remote_analog);
    if (drv_right_hand_analog_.isValid())
    {
        yInfo() << log_ID_ << "Right arm analogsensorclient succefully opened.";

        if (!drv_right_hand_analog_.view(itf_right_hand_analog_))
        {
            yError() << log_ID_ << "Error getting right hand IAnalogSensor interface!";
            throw std::runtime_error("Error getting right hand IAnalogSensor interface!");
        }
    }
    else
    {
        yError() << log_ID_ << "Error opening right arm analogsensorclient device.";
        return false;
    }
#endif

    return true;
}

bool iKinCADSuperimposer::setArmCartesianController()
{
    Property rightarm_cartesian_options;
    rightarm_cartesian_options.put("device", "cartesiancontrollerclient");
    rightarm_cartesian_options.put("local", "/" + ID_ + "/cart_right_arm");
    rightarm_cartesian_options.put("remote", "/" + robot_ + "/cartesianController/right_arm");

    drv_right_arm_cartesian_.open(rightarm_cartesian_options);
    if (drv_right_arm_cartesian_.isValid())
    {
        drv_right_arm_cartesian_.view(itf_right_arm_cart_);
        if (!itf_right_arm_cart_)
        {
            yError() << log_ID_ << "Error getting ICartesianControl interface.";
            return false;
        }
        yInfo() << log_ID_ << "cartesiancontrollerclient succefully opened.";
    }
    else
    {
        yError() << log_ID_ << "Error opening cartesiancontrollerclient device.";
        return false;
    }

    return true;
}

Vector iKinCADSuperimposer::readRootToEE()
{
    Vector enc_arm = getRightArmEncoders();

    Vector root_ee_enc(10);

    root_ee_enc.setSubvector(0, getTorsoEncoders());

    for (size_t i = 0; i < 7; ++i)
        root_ee_enc(i+3) = enc_arm(i);

    return root_ee_enc;
}
<|endoftext|>


<commit_before>// Copyright (c) 2012 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "content/browser/accessibility/browser_accessibility_manager.h"

#include "base/logging.h"
#include "content/browser/accessibility/browser_accessibility.h"
#include "content/common/accessibility_messages.h"

using webkit_glue::WebAccessibility;

BrowserAccessibility* BrowserAccessibilityFactory::Create() {
  return BrowserAccessibility::Create();
}

// Start child IDs at -1 and decrement each time, because clients use
// child IDs of 1, 2, 3, ... to access the children of an object by
// index, so we use negative IDs to clearly distinguish between indices
// and unique IDs.
// static
int32 BrowserAccessibilityManager::next_child_id_ = -1;

#if !defined(OS_MACOSX) && \
    !(defined(OS_WIN) && !defined(USE_AURA)) && \
    !defined(TOOLKIT_GTK)
// We have subclassess of BrowserAccessibilityManager on Mac, Linux/GTK,
// and non-Aura Win. For any other platform, instantiate the base class.
// static
BrowserAccessibilityManager* BrowserAccessibilityManager::Create(
    gfx::NativeView parent_view,
    const WebAccessibility& src,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory) {
  return new BrowserAccessibilityManager(
      parent_view, src, delegate, factory);
}
#endif

// static
BrowserAccessibilityManager* BrowserAccessibilityManager::CreateEmptyDocument(
    gfx::NativeView parent_view,
    WebAccessibility::State state,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory) {
  // Use empty document to process notifications
  webkit_glue::WebAccessibility empty_document;
  empty_document.id = 0;
  empty_document.role = WebAccessibility::ROLE_ROOT_WEB_AREA;
  empty_document.state = state | (1 << WebAccessibility::STATE_READONLY);
  return BrowserAccessibilityManager::Create(
      parent_view, empty_document, delegate, factory);
}

BrowserAccessibilityManager::BrowserAccessibilityManager(
    gfx::NativeView parent_view,
    const WebAccessibility& src,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory)
    : parent_view_(parent_view),
      delegate_(delegate),
      factory_(factory),
      focus_(NULL) {
  root_ = CreateAccessibilityTree(NULL, src, 0, false);
  if (!focus_)
    SetFocus(root_, false);
}

// static
int32 BrowserAccessibilityManager::GetNextChildID() {
  // Get the next child ID, and wrap around when we get near the end
  // of a 32-bit integer range. It's okay to wrap around; we just want
  // to avoid it as long as possible because clients may cache the ID of
  // an object for a while to determine if they've seen it before.
  next_child_id_--;
  if (next_child_id_ == -2000000000)
    next_child_id_ = -1;

  return next_child_id_;
}

BrowserAccessibilityManager::~BrowserAccessibilityManager() {
  // Clients could still hold references to some nodes of the tree, so
  // calling InternalReleaseReference will make sure that as many nodes
  // as possible are released now, and remaining nodes are marked as
  // inactive so that calls to any methods on them will fail gracefully.
  focus_->InternalReleaseReference(false);
  root_->InternalReleaseReference(true);
}

BrowserAccessibility* BrowserAccessibilityManager::GetRoot() {
  return root_;
}

BrowserAccessibility* BrowserAccessibilityManager::GetFromChildID(
    int32 child_id) {
  base::hash_map<int32, BrowserAccessibility*>::iterator iter =
      child_id_map_.find(child_id);
  if (iter != child_id_map_.end()) {
    return iter->second;
  } else {
    return NULL;
  }
}

BrowserAccessibility* BrowserAccessibilityManager::GetFromRendererID(
    int32 renderer_id) {
  base::hash_map<int32, int32>::iterator iter =
      renderer_id_to_child_id_map_.find(renderer_id);
  if (iter == renderer_id_to_child_id_map_.end())
    return NULL;

  int32 child_id = iter->second;
  return GetFromChildID(child_id);
}

void BrowserAccessibilityManager::GotFocus() {
  if (!focus_)
    return;

  NotifyAccessibilityEvent(AccessibilityNotificationFocusChanged, focus_);
}

void BrowserAccessibilityManager::Remove(int32 child_id, int32 renderer_id) {
  child_id_map_.erase(child_id);

  // TODO(ctguil): Investigate if hit. We should never have a newer entry.
  DCHECK(renderer_id_to_child_id_map_[renderer_id] == child_id);
  // Make sure we don't overwrite a newer entry (see UpdateNode for a possible
  // corner case).
  if (renderer_id_to_child_id_map_[renderer_id] == child_id)
    renderer_id_to_child_id_map_.erase(renderer_id);
}

void BrowserAccessibilityManager::OnAccessibilityNotifications(
    const std::vector<AccessibilityHostMsg_NotificationParams>& params) {
  for (uint32 index = 0; index < params.size(); index++) {
    const AccessibilityHostMsg_NotificationParams& param = params[index];

    // Update the tree.
    UpdateNode(param.acc_tree, param.includes_children);

    // Find the node corresponding to the id that's the target of the
    // notification (which may not be the root of the update tree).
    base::hash_map<int32, int32>::iterator iter =
        renderer_id_to_child_id_map_.find(param.id);
    if (iter == renderer_id_to_child_id_map_.end()) {
      continue;
    }
    int32 child_id = iter->second;
    BrowserAccessibility* node = GetFromChildID(child_id);
    if (!node) {
      NOTREACHED();
      continue;
    }

    int notification_type = param.notification_type;
    if (notification_type == AccessibilityNotificationFocusChanged) {
      SetFocus(node, false);

      // Don't send a native focus event if the window itself doesn't
      // have focus.
      if (delegate_ && !delegate_->HasFocus())
        continue;
    }

    // Send the notification event to the operating system.
    NotifyAccessibilityEvent(notification_type, node);

    // Set initial focus when a page is loaded.
    if (notification_type == AccessibilityNotificationLoadComplete) {
      if (!focus_)
        SetFocus(root_, false);
      if (!delegate_ || delegate_->HasFocus())
        NotifyAccessibilityEvent(AccessibilityNotificationFocusChanged, focus_);
    }
  }
}

gfx::NativeView BrowserAccessibilityManager::GetParentView() {
  return parent_view_;
}

BrowserAccessibility* BrowserAccessibilityManager::GetFocus(
    BrowserAccessibility* root) {
  if (focus_ && (!root || focus_->IsDescendantOf(root)))
    return focus_;

  return NULL;
}

void BrowserAccessibilityManager::SetFocus(
    BrowserAccessibility* node, bool notify) {
  if (focus_ != node) {
    if (focus_)
      focus_->InternalReleaseReference(false);
    focus_ = node;
    if (focus_)
      focus_->InternalAddReference();
  }

  if (notify && node && delegate_)
    delegate_->SetAccessibilityFocus(node->renderer_id());
}

void BrowserAccessibilityManager::DoDefaultAction(
    const BrowserAccessibility& node) {
  if (delegate_)
    delegate_->AccessibilityDoDefaultAction(node.renderer_id());
}

void BrowserAccessibilityManager::ScrollToMakeVisible(
    const BrowserAccessibility& node, gfx::Rect subfocus) {
  if (delegate_) {
    delegate_->AccessibilityScrollToMakeVisible(node.renderer_id(), subfocus);
  }
}

void BrowserAccessibilityManager::ScrollToPoint(
    const BrowserAccessibility& node, gfx::Point point) {
  if (delegate_) {
    delegate_->AccessibilityScrollToPoint(node.renderer_id(), point);
  }
}

void BrowserAccessibilityManager::SetTextSelection(
    const BrowserAccessibility& node, int start_offset, int end_offset) {
  if (delegate_) {
    delegate_->AccessibilitySetTextSelection(
        node.renderer_id(), start_offset, end_offset);
  }
}

gfx::Rect BrowserAccessibilityManager::GetViewBounds() {
  if (delegate_)
    return delegate_->GetViewBounds();
  return gfx::Rect();
}

void BrowserAccessibilityManager::UpdateNode(
    const WebAccessibility& src,
    bool include_children) {
  BrowserAccessibility* current = NULL;

  // Look for the node to replace. Either we're replacing the whole tree
  // (role is ROOT_WEB_AREA) or we look it up based on its renderer ID.
  if (src.role == WebAccessibility::ROLE_ROOT_WEB_AREA) {
    current = root_;
  } else {
    base::hash_map<int32, int32>::iterator iter =
        renderer_id_to_child_id_map_.find(src.id);
    if (iter != renderer_id_to_child_id_map_.end()) {
      int32 child_id = iter->second;
      current = GetFromChildID(child_id);
    }
  }

  // If we can't find the node to replace, we're out of sync with the
  // renderer (this would be a bug).
  DCHECK(current);
  if (!current)
    return;

  // If this update is just for a single node (|include_children| is false),
  // modify |current| directly and return - no tree changes are needed.
  if (!include_children) {
    DCHECK_EQ(0U, src.children.size());
    current->PreInitialize(
        this,
        current->parent(),
        current->child_id(),
        current->index_in_parent(),
        src);
    current->PostInitialize();
    return;
  }

  BrowserAccessibility* current_parent = current->parent();
  int current_index_in_parent = current->index_in_parent();

  // Detach all of the nodes in the old tree and get a single flat vector
  // of all node pointers.
  std::vector<BrowserAccessibility*> old_tree_nodes;
  current->DetachTree(&old_tree_nodes);

  // Build a new tree, reusing old nodes if possible. Each node that's
  // reused will have its reference count incremented by one.
  current = CreateAccessibilityTree(
      current_parent, src, current_index_in_parent, true);

  // Decrement the reference count of all nodes in the old tree, which will
  // delete any nodes no longer needed.
  for (int i = 0; i < static_cast<int>(old_tree_nodes.size()); i++)
    old_tree_nodes[i]->InternalReleaseReference(false);

  // If the only reference to the focused node is focus_ itself, then the
  // focused node is no longer in the tree, so set the focus to the root.
  if (focus_ && focus_->ref_count() == 1) {
    SetFocus(root_, false);

    if (delegate_ && delegate_->HasFocus())
      NotifyAccessibilityEvent(AccessibilityNotificationBlur, focus_);
  }
}

BrowserAccessibility* BrowserAccessibilityManager::CreateAccessibilityTree(
    BrowserAccessibility* parent,
    const WebAccessibility& src,
    int index_in_parent,
    bool send_show_events) {
  BrowserAccessibility* instance = NULL;
  int32 child_id = 0;
  bool children_can_send_show_events = send_show_events;
  base::hash_map<int32, int32>::iterator iter =
      renderer_id_to_child_id_map_.find(src.id);

  // If a BrowserAccessibility instance for this ID already exists, add a
  // new reference to it and retrieve its children vector.
  if (iter != renderer_id_to_child_id_map_.end()) {
    child_id = iter->second;
    instance = GetFromChildID(child_id);
  }

  // If the node has changed roles, don't reuse a BrowserAccessibility
  // object, that could confuse a screen reader.
  // TODO(dtseng): Investigate when this gets hit; See crbug.com/93095.
  DCHECK(!instance || instance->role() == src.role);

  // If we're reusing a node, it should already be detached from a parent
  // and any children. If not, that means we have a serious bug somewhere,
  // like the same child is reachable from two places in the same tree.
  if (instance && (instance->parent() != NULL || instance->child_count() > 0)) {
    NOTREACHED();
    instance = NULL;
  }

  if (instance) {
    // If we're reusing a node, update its parent and increment its
    // reference count.
    instance->UpdateParent(parent, index_in_parent);
    instance->InternalAddReference();
    send_show_events = false;
  } else {
    // Otherwise, create a new instance.
    instance = factory_->Create();
    child_id = GetNextChildID();
    children_can_send_show_events = false;
  }

  instance->PreInitialize(this, parent, child_id, index_in_parent, src);
  child_id_map_[child_id] = instance;
  renderer_id_to_child_id_map_[src.id] = child_id;

  if ((src.state >> WebAccessibility::STATE_FOCUSED) & 1)
    SetFocus(instance, false);

  for (int i = 0; i < static_cast<int>(src.children.size()); ++i) {
    BrowserAccessibility* child = CreateAccessibilityTree(
        instance, src.children[i], i, children_can_send_show_events);
    instance->AddChild(child);
  }

  if (src.role == WebAccessibility::ROLE_ROOT_WEB_AREA)
    root_ = instance;

  // Note: the purpose of send_show_events and children_can_send_show_events
  // is so that we send a single ObjectShow event for the root of a subtree
  // that just appeared for the first time, but not on any descendant of
  // that subtree.
  if (send_show_events)
    NotifyAccessibilityEvent(AccessibilityNotificationObjectShow, instance);

  instance->PostInitialize();

  return instance;
}
<commit_msg>Revert 139628 - Better cross platform behavior for blur.<commit_after>// Copyright (c) 2012 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "content/browser/accessibility/browser_accessibility_manager.h"

#include "base/logging.h"
#include "content/browser/accessibility/browser_accessibility.h"
#include "content/common/accessibility_messages.h"

using webkit_glue::WebAccessibility;

BrowserAccessibility* BrowserAccessibilityFactory::Create() {
  return BrowserAccessibility::Create();
}

// Start child IDs at -1 and decrement each time, because clients use
// child IDs of 1, 2, 3, ... to access the children of an object by
// index, so we use negative IDs to clearly distinguish between indices
// and unique IDs.
// static
int32 BrowserAccessibilityManager::next_child_id_ = -1;

#if !defined(OS_MACOSX) && \
    !(defined(OS_WIN) && !defined(USE_AURA)) && \
    !defined(TOOLKIT_GTK)
// We have subclassess of BrowserAccessibilityManager on Mac, Linux/GTK,
// and non-Aura Win. For any other platform, instantiate the base class.
// static
BrowserAccessibilityManager* BrowserAccessibilityManager::Create(
    gfx::NativeView parent_view,
    const WebAccessibility& src,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory) {
  return new BrowserAccessibilityManager(
      parent_view, src, delegate, factory);
}
#endif

// static
BrowserAccessibilityManager* BrowserAccessibilityManager::CreateEmptyDocument(
    gfx::NativeView parent_view,
    WebAccessibility::State state,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory) {
  // Use empty document to process notifications
  webkit_glue::WebAccessibility empty_document;
  empty_document.id = 0;
  empty_document.role = WebAccessibility::ROLE_ROOT_WEB_AREA;
  empty_document.state = state | (1 << WebAccessibility::STATE_READONLY);
  return BrowserAccessibilityManager::Create(
      parent_view, empty_document, delegate, factory);
}

BrowserAccessibilityManager::BrowserAccessibilityManager(
    gfx::NativeView parent_view,
    const WebAccessibility& src,
    BrowserAccessibilityDelegate* delegate,
    BrowserAccessibilityFactory* factory)
    : parent_view_(parent_view),
      delegate_(delegate),
      factory_(factory),
      focus_(NULL) {
  root_ = CreateAccessibilityTree(NULL, src, 0, false);
  if (!focus_)
    SetFocus(root_, false);
}

// static
int32 BrowserAccessibilityManager::GetNextChildID() {
  // Get the next child ID, and wrap around when we get near the end
  // of a 32-bit integer range. It's okay to wrap around; we just want
  // to avoid it as long as possible because clients may cache the ID of
  // an object for a while to determine if they've seen it before.
  next_child_id_--;
  if (next_child_id_ == -2000000000)
    next_child_id_ = -1;

  return next_child_id_;
}

BrowserAccessibilityManager::~BrowserAccessibilityManager() {
  // Clients could still hold references to some nodes of the tree, so
  // calling InternalReleaseReference will make sure that as many nodes
  // as possible are released now, and remaining nodes are marked as
  // inactive so that calls to any methods on them will fail gracefully.
  focus_->InternalReleaseReference(false);
  root_->InternalReleaseReference(true);
}

BrowserAccessibility* BrowserAccessibilityManager::GetRoot() {
  return root_;
}

BrowserAccessibility* BrowserAccessibilityManager::GetFromChildID(
    int32 child_id) {
  base::hash_map<int32, BrowserAccessibility*>::iterator iter =
      child_id_map_.find(child_id);
  if (iter != child_id_map_.end()) {
    return iter->second;
  } else {
    return NULL;
  }
}

BrowserAccessibility* BrowserAccessibilityManager::GetFromRendererID(
    int32 renderer_id) {
  base::hash_map<int32, int32>::iterator iter =
      renderer_id_to_child_id_map_.find(renderer_id);
  if (iter == renderer_id_to_child_id_map_.end())
    return NULL;

  int32 child_id = iter->second;
  return GetFromChildID(child_id);
}

void BrowserAccessibilityManager::GotFocus() {
  if (!focus_)
    return;

  NotifyAccessibilityEvent(AccessibilityNotificationFocusChanged, focus_);
}

void BrowserAccessibilityManager::Remove(int32 child_id, int32 renderer_id) {
  child_id_map_.erase(child_id);

  // TODO(ctguil): Investigate if hit. We should never have a newer entry.
  DCHECK(renderer_id_to_child_id_map_[renderer_id] == child_id);
  // Make sure we don't overwrite a newer entry (see UpdateNode for a possible
  // corner case).
  if (renderer_id_to_child_id_map_[renderer_id] == child_id)
    renderer_id_to_child_id_map_.erase(renderer_id);
}

void BrowserAccessibilityManager::OnAccessibilityNotifications(
    const std::vector<AccessibilityHostMsg_NotificationParams>& params) {
  for (uint32 index = 0; index < params.size(); index++) {
    const AccessibilityHostMsg_NotificationParams& param = params[index];

    // Update the tree.
    UpdateNode(param.acc_tree, param.includes_children);

    // Find the node corresponding to the id that's the target of the
    // notification (which may not be the root of the update tree).
    base::hash_map<int32, int32>::iterator iter =
        renderer_id_to_child_id_map_.find(param.id);
    if (iter == renderer_id_to_child_id_map_.end()) {
      continue;
    }
    int32 child_id = iter->second;
    BrowserAccessibility* node = GetFromChildID(child_id);
    if (!node) {
      NOTREACHED();
      continue;
    }

    int notification_type = param.notification_type;
    if (notification_type == AccessibilityNotificationFocusChanged ||
        notification_type == AccessibilityNotificationBlur) {
      SetFocus(node, false);

      // Don't send a native focus event if the window itself doesn't
      // have focus.
      if (delegate_ && !delegate_->HasFocus())
        continue;
    }

    // Send the notification event to the operating system.
    NotifyAccessibilityEvent(notification_type, node);

    // Set initial focus when a page is loaded.
    if (notification_type == AccessibilityNotificationLoadComplete) {
      if (!focus_)
        SetFocus(root_, false);
      if (!delegate_ || delegate_->HasFocus())
        NotifyAccessibilityEvent(AccessibilityNotificationFocusChanged, focus_);
    }
  }
}

gfx::NativeView BrowserAccessibilityManager::GetParentView() {
  return parent_view_;
}

BrowserAccessibility* BrowserAccessibilityManager::GetFocus(
    BrowserAccessibility* root) {
  if (focus_ && (!root || focus_->IsDescendantOf(root)))
    return focus_;

  return NULL;
}

void BrowserAccessibilityManager::SetFocus(
    BrowserAccessibility* node, bool notify) {
  if (focus_ != node) {
    if (focus_)
      focus_->InternalReleaseReference(false);
    focus_ = node;
    if (focus_)
      focus_->InternalAddReference();
  }

  if (notify && node && delegate_)
    delegate_->SetAccessibilityFocus(node->renderer_id());
}

void BrowserAccessibilityManager::DoDefaultAction(
    const BrowserAccessibility& node) {
  if (delegate_)
    delegate_->AccessibilityDoDefaultAction(node.renderer_id());
}

void BrowserAccessibilityManager::ScrollToMakeVisible(
    const BrowserAccessibility& node, gfx::Rect subfocus) {
  if (delegate_) {
    delegate_->AccessibilityScrollToMakeVisible(node.renderer_id(), subfocus);
  }
}

void BrowserAccessibilityManager::ScrollToPoint(
    const BrowserAccessibility& node, gfx::Point point) {
  if (delegate_) {
    delegate_->AccessibilityScrollToPoint(node.renderer_id(), point);
  }
}

void BrowserAccessibilityManager::SetTextSelection(
    const BrowserAccessibility& node, int start_offset, int end_offset) {
  if (delegate_) {
    delegate_->AccessibilitySetTextSelection(
        node.renderer_id(), start_offset, end_offset);
  }
}

gfx::Rect BrowserAccessibilityManager::GetViewBounds() {
  if (delegate_)
    return delegate_->GetViewBounds();
  return gfx::Rect();
}

void BrowserAccessibilityManager::UpdateNode(
    const WebAccessibility& src,
    bool include_children) {
  BrowserAccessibility* current = NULL;

  // Look for the node to replace. Either we're replacing the whole tree
  // (role is ROOT_WEB_AREA) or we look it up based on its renderer ID.
  if (src.role == WebAccessibility::ROLE_ROOT_WEB_AREA) {
    current = root_;
  } else {
    base::hash_map<int32, int32>::iterator iter =
        renderer_id_to_child_id_map_.find(src.id);
    if (iter != renderer_id_to_child_id_map_.end()) {
      int32 child_id = iter->second;
      current = GetFromChildID(child_id);
    }
  }

  // If we can't find the node to replace, we're out of sync with the
  // renderer (this would be a bug).
  DCHECK(current);
  if (!current)
    return;

  // If this update is just for a single node (|include_children| is false),
  // modify |current| directly and return - no tree changes are needed.
  if (!include_children) {
    DCHECK_EQ(0U, src.children.size());
    current->PreInitialize(
        this,
        current->parent(),
        current->child_id(),
        current->index_in_parent(),
        src);
    current->PostInitialize();
    return;
  }

  BrowserAccessibility* current_parent = current->parent();
  int current_index_in_parent = current->index_in_parent();

  // Detach all of the nodes in the old tree and get a single flat vector
  // of all node pointers.
  std::vector<BrowserAccessibility*> old_tree_nodes;
  current->DetachTree(&old_tree_nodes);

  // Build a new tree, reusing old nodes if possible. Each node that's
  // reused will have its reference count incremented by one.
  current = CreateAccessibilityTree(
      current_parent, src, current_index_in_parent, true);

  // Decrement the reference count of all nodes in the old tree, which will
  // delete any nodes no longer needed.
  for (int i = 0; i < static_cast<int>(old_tree_nodes.size()); i++)
    old_tree_nodes[i]->InternalReleaseReference(false);

  // If the only reference to the focused node is focus_ itself, then the
  // focused node is no longer in the tree, so set the focus to the root.
  if (focus_ && focus_->ref_count() == 1) {
    SetFocus(root_, false);

    if (delegate_ && delegate_->HasFocus())
      NotifyAccessibilityEvent(AccessibilityNotificationBlur, focus_);
  }
}

BrowserAccessibility* BrowserAccessibilityManager::CreateAccessibilityTree(
    BrowserAccessibility* parent,
    const WebAccessibility& src,
    int index_in_parent,
    bool send_show_events) {
  BrowserAccessibility* instance = NULL;
  int32 child_id = 0;
  bool children_can_send_show_events = send_show_events;
  base::hash_map<int32, int32>::iterator iter =
      renderer_id_to_child_id_map_.find(src.id);

  // If a BrowserAccessibility instance for this ID already exists, add a
  // new reference to it and retrieve its children vector.
  if (iter != renderer_id_to_child_id_map_.end()) {
    child_id = iter->second;
    instance = GetFromChildID(child_id);
  }

  // If the node has changed roles, don't reuse a BrowserAccessibility
  // object, that could confuse a screen reader.
  // TODO(dtseng): Investigate when this gets hit; See crbug.com/93095.
  DCHECK(!instance || instance->role() == src.role);

  // If we're reusing a node, it should already be detached from a parent
  // and any children. If not, that means we have a serious bug somewhere,
  // like the same child is reachable from two places in the same tree.
  if (instance && (instance->parent() != NULL || instance->child_count() > 0)) {
    NOTREACHED();
    instance = NULL;
  }

  if (instance) {
    // If we're reusing a node, update its parent and increment its
    // reference count.
    instance->UpdateParent(parent, index_in_parent);
    instance->InternalAddReference();
    send_show_events = false;
  } else {
    // Otherwise, create a new instance.
    instance = factory_->Create();
    child_id = GetNextChildID();
    children_can_send_show_events = false;
  }

  instance->PreInitialize(this, parent, child_id, index_in_parent, src);
  child_id_map_[child_id] = instance;
  renderer_id_to_child_id_map_[src.id] = child_id;

  if ((src.state >> WebAccessibility::STATE_FOCUSED) & 1)
    SetFocus(instance, false);

  for (int i = 0; i < static_cast<int>(src.children.size()); ++i) {
    BrowserAccessibility* child = CreateAccessibilityTree(
        instance, src.children[i], i, children_can_send_show_events);
    instance->AddChild(child);
  }

  if (src.role == WebAccessibility::ROLE_ROOT_WEB_AREA)
    root_ = instance;

  // Note: the purpose of send_show_events and children_can_send_show_events
  // is so that we send a single ObjectShow event for the root of a subtree
  // that just appeared for the first time, but not on any descendant of
  // that subtree.
  if (send_show_events)
    NotifyAccessibilityEvent(AccessibilityNotificationObjectShow, instance);

  instance->PostInitialize();

  return instance;
}
<|endoftext|>


<commit_before>#include <string>
#include "dynamicsJRLJapan/Joint.h"
#include "dynamicsJRLJapan/HumanoidDynamicMultiBody.h"
#include "robotDynamics/jrlRobotDynamicsObjectConstructor.h"

using namespace std;
using namespace dynamicsJRLJapan;

void DisplayBody(CjrlBody *aBody)
{
  matrix3d InertiaMatrix = aBody->inertiaMatrix();

  cout << "Attached body:" << endl;
  cout << "Mass of the attached body: " << aBody->mass() << endl;
  cout << "Local center of mass: " << aBody->localCenterOfMass() << endl;
  cout << "Inertia matrix:" << endl;
  cout << InertiaMatrix << endl;

}
void DisplayJoint(Joint * aJoint)
{
  cout << "Joint : " << aJoint->getName() << endl;
  unsigned int NbCJ = aJoint->countChildJoints();
  if (aJoint->parentJoint()!=0)
    cout << "Father: " << ((Joint *)(aJoint->parentJoint()))->getName() << endl;
  cout << "Number of children : " << NbCJ << endl;
  for(unsigned int li=0;li<NbCJ;li++)
    cout << "Child("<<li<<")="
	 << ((Joint*)(aJoint->childJoint(li)))->getName() << endl;

  cout << "Current transformation " << aJoint->currentTransformation() << endl;

}

void RecursiveDisplayOfJoints(CjrlJoint *aJoint)
{
  if (aJoint==0)
    return;

  Joint *a2Joint=0;

  int NbChildren = aJoint->countChildJoints();
  if (NbChildren == 0)
    return ;

  a2Joint = dynamic_cast<Joint *>( aJoint);
  if (a2Joint==0)
    return;

  DisplayJoint(a2Joint);
  //DisplayBody(aJoint->linkedBody());
  for(int i=0;i<NbChildren;i++)
    {
      // Returns a const so we have to force the casting/
      RecursiveDisplayOfJoints((CjrlJoint *)a2Joint->childJoint(i));
    }
  //cout << " End for Joint: " << a2Joint->getName() << endl;
}

void recursiveMultibodyCopy(Joint *initJoint, Joint * copyJoint)
{
  int lNbOfChildren= initJoint->countChildJoints();

  cout << "nbre of childs : " << lNbOfChildren << endl ;

  // stop test
  if (lNbOfChildren == 0) return ;

  for(int li=0;li<lNbOfChildren;li++)
    {

      Joint *Child = (Joint *)initJoint->childJoint(li);
      if (Child!=0)
	{
	  Joint *a2CopyJoint ;
	  a2CopyJoint = new Joint(*Child);
	  copyJoint->addChildJoint(*a2CopyJoint);
	  a2CopyJoint->SetFatherJoint(copyJoint);
	  recursiveMultibodyCopy(Child, a2CopyJoint) ;
	}
    }
}

void PerformCopyFromJointsTree(HumanoidDynamicMultiBody *aHDR,
			       HumanoidDynamicMultiBody *a2HDR)
{
  Joint * InitJoint = (Joint *)aHDR->rootJoint() ;

  Joint * CopyJoint = new Joint(*InitJoint);

  a2HDR->rootJoint(*CopyJoint);

  recursiveMultibodyCopy(InitJoint, CopyJoint) ;

  // Initialize the second humanoid from the joint tree.

  std::vector<NameAndRank_t> LinkJointNameAndRank;
  aHDR->getLinksBetweenJointNamesAndRank(LinkJointNameAndRank);
  a2HDR->setLinksBetweenJointNamesAndRank(LinkJointNameAndRank);

  RecursiveDisplayOfJoints(a2HDR->rootJoint());

  cout << " ============================== AT END 1 ======================================= " <<
    endl << endl << endl ;

  for(unsigned int i=0 ; i < LinkJointNameAndRank.size() ; i++ )
    {
      cout << "NAME : " << LinkJointNameAndRank[i].LinkName << "\t rank : " << LinkJointNameAndRank[i].RankInConfiguration << endl ;
    }

  a2HDR->InitializeFromJointsTree();

  RecursiveDisplayOfJoints(a2HDR->rootJoint());

  cout << " ============================== AT END 2 ======================================= " <<
    endl << endl << endl ;

  // Copy the bodies.
  std::vector<CjrlJoint *> VecOfInitJoints = aHDR->jointVector();
  std::vector<CjrlJoint *> VecOfCopyJoints = a2HDR->jointVector();

  if (VecOfInitJoints.size()!=VecOfCopyJoints.size())
    {
      std::cout << "Problem while copying the joints. " <<endl;
      exit(-1);
    }

  for(unsigned int i=0;i<VecOfInitJoints.size();i++)
    {
      if ((VecOfCopyJoints[i]!=0) &&
	  (VecOfInitJoints[i]!=0))
      *((DynamicBody *)VecOfCopyJoints[i]->linkedBody()) =
	*((DynamicBody *)VecOfInitJoints[i]->linkedBody());
    }

}

int main(int argc, char *argv[])
{
  int VerboseMode = 3;

  string aPath="/home/mpoirier/openrobots/OpenHRP/etc/HRP2JRL/";
  string aName="HRP2JRLmain.wrl";
  string JointToRank = "/home/mpoirier/openrobots/share/walkgenjrl/HRP2LinkJointRank.xml";
  string aSpecificitiesFileName =  "/home/mpoirier/openrobots/share/walkgenjrl/HRP2Specificities.xml";

  int argindex=1;
  if (argc>1)
    {
      aPath=argv[argindex++];
      argc--;
    }
  if (argc>1)
    {
      aName=argv[argindex++];
      argc--;
    }
  if (argc>1)
    {
      JointToRank=argv[argindex++];
      argc--;
    }
  if (argc>1)
    {
      aSpecificitiesFileName=argv[argindex++];
      argc--;
    }

  CjrlRobotDynamicsObjectConstructor <
  dynamicsJRLJapan::DynamicMultiBody,
    dynamicsJRLJapan::HumanoidDynamicMultiBody,
    dynamicsJRLJapan::JointFreeflyer,
    dynamicsJRLJapan::JointRotation,
    dynamicsJRLJapan::JointTranslation,
    dynamicsJRLJapan::Body >  aRobotDynamicsObjectConstructor;

  // Read the first humanoid.
  HumanoidDynamicMultiBody * aHDR =
    dynamic_cast<HumanoidDynamicMultiBody*>(aRobotDynamicsObjectConstructor.createhumanoidDynamicRobot());

  aHDR->parserVRML(aPath,
		   aName,
		   (char *)JointToRank.c_str());

  aHDR->SetHumanoidSpecificitiesFile(aSpecificitiesFileName);

  // Pretend to build the second humanoid.
  HumanoidDynamicMultiBody *a2HDR =
    dynamic_cast<HumanoidDynamicMultiBody*>(aRobotDynamicsObjectConstructor.createhumanoidDynamicRobot());

  PerformCopyFromJointsTree(aHDR, a2HDR);

#if 0
  // Test the new humanoid structure.
  double dInitPos[40] = {
    0.0, 0.0, -26.0, 50.0, -24.0, 0.0, 0.0, 0.0, -26.0, 50.0, -24.0, 0.0,  // legs

    0.0, 0.0, 0.0, 0.0, // chest and head

    15.0, -10.0, 0.0, -30.0, 0.0, 0.0, 10.0, // right arm
    15.0,  10.0, 0.0, -30.0, 0.0, 0.0, 10.0, // left arm

    -20.0, 20.0, -20.0, 20.0, -20.0, // right hand
    -10.0, 10.0, -10.0, 10.0, -10.0  // left hand
  };

  // This is mandatory for this implementation of computeForwardKinematics
  // to compute the derivative of the momentum.
  aHDR->SetTimeStep(0.005);
  aHDR->setComputeAcceleration(false);
  aHDR->setComputeBackwardDynamics(false);
  aHDR->setComputeZMP(true);

  a2HDR->SetTimeStep(0.005);
  a2HDR->setComputeAcceleration(false);
  a2HDR->setComputeBackwardDynamics(false);
  a2HDR->setComputeZMP(true);

  int NbOfDofs = a2HDR->numberDof();
  if (VerboseMode>2)
    std::cout << "NbOfDofs :" << NbOfDofs << std::endl;

  MAL_VECTOR_DIM(aCurrentConf,double,NbOfDofs);
  int lindex=0;
  for(int i=0;i<6;i++)
    aCurrentConf[lindex++] = 0.0;

  for(int i=0;i<(NbOfDofs-6 < 40 ? NbOfDofs-6 : 40) ;i++)
    aCurrentConf[lindex++] = dInitPos[i]*M_PI/180.0;

  aHDR->currentConfiguration(aCurrentConf);
  a2HDR->currentConfiguration(aCurrentConf);

  MAL_VECTOR_DIM(aCurrentVel,double,NbOfDofs);
  lindex=0;
  for(int i=0;i<NbOfDofs;i++)
    aCurrentVel[lindex++] = 0.0;

  MAL_VECTOR_DIM(aCurrentAcc,double,NbOfDofs);
  MAL_VECTOR_FILL(aCurrentAcc,0.0);

  MAL_S3_VECTOR(ZMPval,double);

  for(int i=0;i<4;i++)
    {
      aHDR->currentVelocity(aCurrentVel);
      aHDR->currentAcceleration(aCurrentAcc);
      aHDR->computeForwardKinematics();
      ZMPval = aHDR->zeroMomentumPoint();
      if (VerboseMode>4)
	{
	  cout << i << "-th value of ZMP : " << ZMPval <<endl;
	  cout << "Should be equal to the CoM: " << aHDR->positionCenterOfMass() << endl;
	}

      a2HDR->currentVelocity(aCurrentVel);
      a2HDR->currentAcceleration(aCurrentAcc);
      a2HDR->computeForwardKinematics();
      ZMPval = a2HDR->zeroMomentumPoint();
      if(VerboseMode>4)
	{
	  cout << i << "-th value of ZMP : " << ZMPval <<endl;
	  cout << "Should be equal to the CoM: " << aHDR->positionCenterOfMass() << endl;
	}

    }

#endif
  if (VerboseMode>2)
    RecursiveDisplayOfJoints(a2HDR->rootJoint());

  delete aHDR;
  delete a2HDR;

}
<commit_msg>--minor bug in displaying joint<commit_after>#include <string>
#include "dynamicsJRLJapan/Joint.h"
#include "dynamicsJRLJapan/HumanoidDynamicMultiBody.h"
#include "robotDynamics/jrlRobotDynamicsObjectConstructor.h"

using namespace std;
using namespace dynamicsJRLJapan;

void DisplayBody(CjrlBody *aBody)
{
  matrix3d InertiaMatrix = aBody->inertiaMatrix();

  cout << "Attached body:" << endl;
  cout << "Mass of the attached body: " << aBody->mass() << endl;
  cout << "Local center of mass: " << aBody->localCenterOfMass() << endl;
  cout << "Inertia matrix:" << endl;
  cout << InertiaMatrix << endl;

}
void DisplayJoint(Joint * aJoint)
{
  cout << "Joint : " << aJoint->getName() << endl;
  unsigned int NbCJ = aJoint->countChildJoints();
  if (aJoint->parentJoint()!=0)
    cout << "Father: " << ((Joint *)(aJoint->parentJoint()))->getName() << endl;
  cout << "Number of children : " << NbCJ << endl;
  for(unsigned int li=0;li<NbCJ;li++)
    cout << "Child("<<li<<")="
	 << ((Joint*)(aJoint->childJoint(li)))->getName() << endl;

  cout << "Current transformation " << aJoint->currentTransformation() << endl;

}

void RecursiveDisplayOfJoints(CjrlJoint *aJoint)
{
  if (aJoint==0)
    return;

  Joint *a2Joint=0;

  int NbChildren = aJoint->countChildJoints();

  a2Joint = dynamic_cast<Joint *>( aJoint);
  if (a2Joint==0)
    return;

  DisplayJoint(a2Joint);
  //DisplayBody(aJoint->linkedBody());
  for(int i=0;i<NbChildren;i++)
    {
      // Returns a const so we have to force the casting/
      RecursiveDisplayOfJoints((CjrlJoint *)a2Joint->childJoint(i));
    }
  //cout << " End for Joint: " << a2Joint->getName() << endl;
}

void recursiveMultibodyCopy(Joint *initJoint, Joint * copyJoint)
{
  int lNbOfChildren= initJoint->countChildJoints();

  cout << "nbre of childs : " << lNbOfChildren << endl ;

  // stop test
  if (lNbOfChildren == 0) return ;

  for(int li=0;li<lNbOfChildren;li++)
    {

      Joint *Child = (Joint *)initJoint->childJoint(li);
      if (Child!=0)
	{
	  Joint *a2CopyJoint ;
	  a2CopyJoint = new Joint(*Child);
	  copyJoint->addChildJoint(*a2CopyJoint);
	  a2CopyJoint->SetFatherJoint(copyJoint);
	  recursiveMultibodyCopy(Child, a2CopyJoint) ;
	}
    }
}

void PerformCopyFromJointsTree(HumanoidDynamicMultiBody *aHDR,
			       HumanoidDynamicMultiBody *a2HDR)
{
  Joint * InitJoint = (Joint *)aHDR->rootJoint() ;

  Joint * CopyJoint = new Joint(*InitJoint);

  a2HDR->rootJoint(*CopyJoint);

  recursiveMultibodyCopy(InitJoint, CopyJoint) ;

  // Initialize the second humanoid from the joint tree.

  std::vector<NameAndRank_t> LinkJointNameAndRank;
  aHDR->getLinksBetweenJointNamesAndRank(LinkJointNameAndRank);
  a2HDR->setLinksBetweenJointNamesAndRank(LinkJointNameAndRank);

  RecursiveDisplayOfJoints(a2HDR->rootJoint());

  cout << " ============================== AT END 1 ======================================= " <<
    endl << endl << endl ;

  for(unsigned int i=0 ; i < LinkJointNameAndRank.size() ; i++ )
    {
      cout << "NAME : " << LinkJointNameAndRank[i].LinkName << "\t rank : " << LinkJointNameAndRank[i].RankInConfiguration << endl ;
    }

  a2HDR->InitializeFromJointsTree();

  RecursiveDisplayOfJoints(a2HDR->rootJoint());

  cout << " ============================== AT END 2 ======================================= " <<
    endl << endl << endl ;

  // Copy the bodies.
  std::vector<CjrlJoint *> VecOfInitJoints = aHDR->jointVector();
  std::vector<CjrlJoint *> VecOfCopyJoints = a2HDR->jointVector();

  if (VecOfInitJoints.size()!=VecOfCopyJoints.size())
    {
      std::cout << "Problem while copying the joints. " <<endl;
      exit(-1);
    }

  for(unsigned int i=0;i<VecOfInitJoints.size();i++)
    {
      if ((VecOfCopyJoints[i]!=0) &&
	  (VecOfInitJoints[i]!=0))
      *((DynamicBody *)VecOfCopyJoints[i]->linkedBody()) =
	*((DynamicBody *)VecOfInitJoints[i]->linkedBody());
    }

}

int main(int argc, char *argv[])
{
  int VerboseMode = 3;

  string aPath="/home/mpoirier/openrobots/OpenHRP/etc/HRP2JRL/";
  string aName="HRP2JRLmain.wrl";
  string JointToRank = "/home/mpoirier/openrobots/share/walkgenjrl/HRP2LinkJointRank.xml";
  string aSpecificitiesFileName =  "/home/mpoirier/openrobots/share/walkgenjrl/HRP2Specificities.xml";

  int argindex=1;
  if (argc>1)
    {
      aPath=argv[argindex++];
      argc--;
    }
  if (argc>1)
    {
      aName=argv[argindex++];
      argc--;
    }
  if (argc>1)
    {
      JointToRank=argv[argindex++];
      argc--;
    }
  if (argc>1)
    {
      aSpecificitiesFileName=argv[argindex++];
      argc--;
    }

  CjrlRobotDynamicsObjectConstructor <
  dynamicsJRLJapan::DynamicMultiBody,
    dynamicsJRLJapan::HumanoidDynamicMultiBody,
    dynamicsJRLJapan::JointFreeflyer,
    dynamicsJRLJapan::JointRotation,
    dynamicsJRLJapan::JointTranslation,
    dynamicsJRLJapan::Body >  aRobotDynamicsObjectConstructor;

  // Read the first humanoid.
  HumanoidDynamicMultiBody * aHDR =
    dynamic_cast<HumanoidDynamicMultiBody*>(aRobotDynamicsObjectConstructor.createhumanoidDynamicRobot());

  aHDR->parserVRML(aPath,
		   aName,
		   (char *)JointToRank.c_str());

  aHDR->SetHumanoidSpecificitiesFile(aSpecificitiesFileName);

  // Pretend to build the second humanoid.
  HumanoidDynamicMultiBody *a2HDR =
    dynamic_cast<HumanoidDynamicMultiBody*>(aRobotDynamicsObjectConstructor.createhumanoidDynamicRobot());

  PerformCopyFromJointsTree(aHDR, a2HDR);

#if 0
  // Test the new humanoid structure.
  double dInitPos[40] = {
    0.0, 0.0, -26.0, 50.0, -24.0, 0.0, 0.0, 0.0, -26.0, 50.0, -24.0, 0.0,  // legs

    0.0, 0.0, 0.0, 0.0, // chest and head

    15.0, -10.0, 0.0, -30.0, 0.0, 0.0, 10.0, // right arm
    15.0,  10.0, 0.0, -30.0, 0.0, 0.0, 10.0, // left arm

    -20.0, 20.0, -20.0, 20.0, -20.0, // right hand
    -10.0, 10.0, -10.0, 10.0, -10.0  // left hand
  };

  // This is mandatory for this implementation of computeForwardKinematics
  // to compute the derivative of the momentum.
  aHDR->SetTimeStep(0.005);
  aHDR->setComputeAcceleration(false);
  aHDR->setComputeBackwardDynamics(false);
  aHDR->setComputeZMP(true);

  a2HDR->SetTimeStep(0.005);
  a2HDR->setComputeAcceleration(false);
  a2HDR->setComputeBackwardDynamics(false);
  a2HDR->setComputeZMP(true);

  int NbOfDofs = a2HDR->numberDof();
  if (VerboseMode>2)
    std::cout << "NbOfDofs :" << NbOfDofs << std::endl;

  MAL_VECTOR_DIM(aCurrentConf,double,NbOfDofs);
  int lindex=0;
  for(int i=0;i<6;i++)
    aCurrentConf[lindex++] = 0.0;

  for(int i=0;i<(NbOfDofs-6 < 40 ? NbOfDofs-6 : 40) ;i++)
    aCurrentConf[lindex++] = dInitPos[i]*M_PI/180.0;

  aHDR->currentConfiguration(aCurrentConf);
  a2HDR->currentConfiguration(aCurrentConf);

  MAL_VECTOR_DIM(aCurrentVel,double,NbOfDofs);
  lindex=0;
  for(int i=0;i<NbOfDofs;i++)
    aCurrentVel[lindex++] = 0.0;

  MAL_VECTOR_DIM(aCurrentAcc,double,NbOfDofs);
  MAL_VECTOR_FILL(aCurrentAcc,0.0);

  MAL_S3_VECTOR(ZMPval,double);

  for(int i=0;i<4;i++)
    {
      aHDR->currentVelocity(aCurrentVel);
      aHDR->currentAcceleration(aCurrentAcc);
      aHDR->computeForwardKinematics();
      ZMPval = aHDR->zeroMomentumPoint();
      if (VerboseMode>4)
	{
	  cout << i << "-th value of ZMP : " << ZMPval <<endl;
	  cout << "Should be equal to the CoM: " << aHDR->positionCenterOfMass() << endl;
	}

      a2HDR->currentVelocity(aCurrentVel);
      a2HDR->currentAcceleration(aCurrentAcc);
      a2HDR->computeForwardKinematics();
      ZMPval = a2HDR->zeroMomentumPoint();
      if(VerboseMode>4)
	{
	  cout << i << "-th value of ZMP : " << ZMPval <<endl;
	  cout << "Should be equal to the CoM: " << aHDR->positionCenterOfMass() << endl;
	}

    }

#endif
  if (VerboseMode>2)
    // RecursiveDisplayOfJoints(a2HDR->rootJoint());
  delete aHDR;
  delete a2HDR;

}
<|endoftext|>