bool Animation::getIK(BVHNode* node)
{
QString jointName=node->name();
if(jointName=="lHand" ||
jointName=="lForeArm" ||
jointName=="lShldr" ||
jointName=="lCollar")
{
return getIK(IK_LHAND);
}
else if(jointName=="rHand" ||
jointName=="rForeArm" ||
jointName=="rShldr" ||
jointName=="rCollar")
{
return getIK(IK_RHAND);
}
else if(jointName=="lThigh" ||
jointName=="lShin" ||
jointName=="lFoot")
{
return getIK(IK_LFOOT);
}
else if(jointName=="rThigh" ||
jointName=="rShin" ||
jointName=="rFoot")
{
return getIK(IK_RFOOT);
}
return false;
}
RTC::ReturnCode_t HockeyArmController::onExecute(RTC::UniqueId ec_id)
{
;
// output the arm position
static signed long prevArmX = 0;
static signed long prevArmY = 0;
signed long armX = g_posController[0].getEncCount();
signed long armY = g_posController[1].getEncCount();
if (armX != prevArmX || armY != prevArmY) {
coil::TimeValue tm(coil::gettimeofday());
m_ArmXY_out.tm.sec = tm.sec();
m_ArmXY_out.tm.nsec = tm.usec() * 1000;
CvPoint2D64f th = {
pulse2rad((armX-INIT_ENC_X)/MOTOR_X_RATIO, 2500*ENCODER_EVAL) + M_PI/2.0,
pulse2rad((armY-INIT_ENC_Y)/MOTOR_Y_RATIO, 2500*ENCODER_EVAL)
};
CvPoint2D64f xy = getFK(th, ARM_1LEN, ARM_2LEN);
m_ArmXY_out.data[0] = xy.x;
m_ArmXY_out.data[1] = xy.y - ARM_OFFSET - HKY_H/2.0;
m_ArmXY_outOut.write();
}
if (!m_armXY_inIn.isNew()) return RTC::RTC_OK;
m_armXY_inIn.read();
CvPoint2D64f xy = {
m_armXY_in.data[0],
m_armXY_in.data[1] + ARM_OFFSET + HKY_H/2.0
};
if (!isArmMovable(xy, ARM_1LEN*0.97, ARM_2LEN*0.97, HKY_W, ARM_MLT_R*1.03, ARM_OFFSET))
return RTC::RTC_OK;
CvPoint2D64f th = getIK(xy, ARM_1LEN, ARM_2LEN, true);
g_posController[0].moveTo(rad2pulse(th.x - M_PI/2, 2500*ENCODER_EVAL) * MOTOR_X_RATIO + INIT_ENC_X, 1000);
g_posController[1].moveTo(rad2pulse(th.y , 2500*ENCODER_EVAL) * MOTOR_Y_RATIO + INIT_ENC_Y, 800);
//std::cout << m_armXY_in.data[0] << ", " << m_armXY_in.data[1] << std::endl;
return RTC::RTC_OK;
}
//==============================================================================
const std::shared_ptr<InverseKinematics>& JacobianNode::getOrCreateIK()
{
return getIK(true);
}
#include <opencv2/opencv.hpp>
#include "HockeyDefinitions.hpp"
#include "PositionController.hpp"
#include "DeviceManager.hpp"
#include "MyMath.hpp"
/* --- グローバル変数 --- */
static unsigned char g_opened[2] = {0x00, 0x00}; // A-IOボードのオープンフラグ
PositionController g_posController[2]; // 位置決め制御クラスのインスタンス
// アームの初期位置
//const CvPoint2D64f g_initXY = {0.0, ARM_1LEN + ARM_2LEN - 50};
//const CvPoint2D64f g_initXY = {0.0, 280+203};
const CvPoint2D64f g_initXY = {0.0, 300+203};
const CvPoint2D64f g_initTh = getIK(g_initXY, ARM_1LEN, ARM_2LEN);
static void clearEncoderCountWithZ(PositionController &p1, PositionController &p2)
{
p1.encoderInit(false); usleep(100000);
p2.encoderInit(false); usleep(100000);
fprintf(stderr, "Z clear OK!\n");
p1.moveTo(INIT_ENC_X, 500);
p2.moveTo(INIT_ENC_Y, 500);
// while (!p1.isMoved() || !p2.isMoved()) { usleep(100); }
//
// p1.servoOff(); p2.servoOff();
// p1.encoderInit(true);
// p2.encoderInit(true);
