source/viewer/instructions/GetJointTorque.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2007, 2008, 2009, University of Karlsruhe (for members see AUTHORS file)
 *
 * ISC License
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 * Created by Alexander Bierbaum
 */

#include "../include/IPSA/ipsaclasses.h"
#include "../include/IPSA/viewer/instructions/GetJointTorque.h"

#include <sstream>

Instruction::tExecResultType GetJointTorque::vExecute(std::vector<std::string>& Instr, std::ostringstream& answer, unsigned int& numberOfReturnValues)
{
    numberOfReturnValues = 0;
                tExecResultType res = eJOINT_NOT_FOUND;
                SoJoint* joint = Instruction::GetNodeOfTypeByName<SoJoint*>(Instr[1]);
                if (joint)
                {
                                SbVec3f torque;
                                if (Instr[2]=="1")
                                                torque = joint->getTorque(2).getValue();
                                else
                                                torque = joint->getTorque(1).getValue();
                                answer << std::scientific << torque[0] << ' ' << torque[1] << ' ' << torque[2];
                                numberOfReturnValues = 3;
                                res = eOK;
                }
                else
                                res = eJOINT_TYPE_INCOMPATIBLE;

                answer  << '\r' << std::endl;
                return res;
}

---