package jp.co.yaskawa.rtc.integratedMotion;
// -*-Java-*-
/*!
 * @file  IntegratedMotionInterfaceSVC_impl.java
 * @brief Service implementation code of IntegratedMotionInterface.idl
 *
 */
/*!
 * @class IntegratedMotionInterfaceSVC_impl
 * Example class implementing IDL interface IntegratedMotionInterface
 */
import org.omg.CORBA.IntHolder;

import jp.co.yaskawa.rtc.integratedMotion.controller.*;
import jp.co.yaskawa.rtc.integratedMotion.module.Buff;
import jp.co.yaskawa.rtc.integratedMotion.module.IntegratedMotionCommand;
import jp.co.yaskawa.rtc.integratedMotion.module.IntegratedMotionInterpolator;
import jp.co.yaskawa.rtc.integratedMotion.module.UnitOperation;
import jp.co.yaskawa.rtc.integratedMotion.module.motion.UnitCommand;

public class IntegratedMotionInterfaceSVC_impl extends IntegratedMotionInterfacePOA{

	UnitOperation unitOperator_;
	IntegratedMotionInterpolator imInterpolator_;
	Buff<UnitCommand<IntegratedMotionCommand>> commandQ_;
	
    public IntegratedMotionInterfaceSVC_impl(IntegratedMotionInterpolator u, UnitOperation o) {
        // Please add extra constructor code here.
    	imInterpolator_ = u;
    	unitOperator_ = o;
    	commandQ_ = imInterpolator_.getQ();    	
    }

    /*
     * Methods corresponding to IDL attributes and operations
     */

    /**
     * 即停止
     */
    public ReturnID abort() {
    	ReturnID rtn = new ReturnID();    	
    	rtn = unitOperator_.abort();    	
    	return rtn;
    }

    /**
     * アラームクリア
     */    
    public ReturnID clearAlarms() {
    	ReturnID rtn = new ReturnID();  	
    	rtn = unitOperator_.clearAlarms();
    	return rtn;
    }

    /**
     * グリッパを閉じる
     */    
    public ReturnID closeGripper(RightLeft type) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID closeGripper(RightLeft type)>"
    	ReturnID rtn = new ReturnID();
    	return rtn;
    }

    /**
     * 発生中のアラームを取得する
     * @param [out] alarms アラーム情報
     */    
    public ReturnID getActiveAlarm(AlarmSeqHolder alarms) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID getActiveAlarm(AlarmSeq alarms)>"
    	ReturnID rtn = new ReturnID();
    	return rtn;
    }

    /**
     * 制御点のフィードバック位置を取得する（直交座標）
     * @param [in] type 右or左アーム
     * @param [in] frameType 原点のフレーム種別
     * @param [out] pos フィードバック位置
     */    
    public ReturnID getFeedbackPosCartesian(RightLeft type, FrameType frameType, CarPosWithElbowHolder pos) {
    	ReturnID rtn = new ReturnID(0,"");
    	
		CarPosWithElbowHolder cPosition = new CarPosWithElbowHolder();
		double[][] carPos = new double[3][4];		
		CarPosWithElbow cPosElbow = new CarPosWithElbow();
		cPosElbow.carPos = carPos;
		cPosElbow.elbow = 0.0;		
		cPosition.value = cPosElbow;

    	unitOperator_.getFeedbackPosCartesian(type, frameType, cPosition);
		
		pos.value = cPosition.value;
		    	
    	return rtn;
    }

    public ReturnID getFeedbackPosJoint(UnitType type, DoubleSeqHolder position) {
    	ReturnID rtn = new ReturnID(0,"");
    	double[] pos = new double[7];		

    	unitOperator_.getFeedbackPosJoint(type, pos);		
		position.value = pos;
		    	
    	return rtn;
    }

    public ReturnID getGripperPos(RightLeft type, org.omg.CORBA.IntHolder angleRatio) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID getGripperPos(RightLeft type, org.omg.CORBA.IntHolder angleRatio)>"
        return null;
    }

    /**
     * 汎用モーションRTCの状態を取得する
     * @param [out] statusId 状態コード
     * @param [out] message 状態文字列
     */
    public ReturnID getState(org.omg.CORBA.IntHolder statusId, org.omg.CORBA.StringHolder message) {
    	ReturnID rtn = new ReturnID();
    	rtn.id = 0;
    	
    	String state = imInterpolator_.getState();
    	
    	if(state.equals("Off") || state.equals("Unpowered") || state.equals("Powered")) {
    		statusId.value = 0x010;
    		message.value = "Unpowered";
    	}

    	if(state.equals("Ready")) {
    		statusId.value = 0x012;
    		message.value = "Ready";
    	}

    	if(state.equals("Busy")) {
    		statusId.value = 0x013;
    		message.value = "Busy";
    	}

    	if(state.equals("Pause")) {
    		statusId.value = 0x014;
    		message.value = "Pause";
    	}
    	
    	if(state.equals("Alarm")) {
    		statusId.value = 0x015;
    		message.value = "Alarm";
    	}
    	
    	return rtn;
    }

    /**
     * 汎用モーションRTCのバージョンを取得する
     * @param [out] バージョン 
     */    
    public ReturnID getVersion(org.omg.CORBA.StringHolder version) {
    	ReturnID rtn = new ReturnID();
    	rtn = unitOperator_.getVersion(version);
    	return rtn;
    }

    /**
     * モーション動作中か否かを取得する
     * @return true:モーション動作中　false:停止中
     */    
    public boolean isMoving() {
    	String state = imInterpolator_.getState();
    	if(state.equals("Busy")|| state.equals("Pause")) {
    		return true;
    	}
        return false;
    }

    /**
     * サーボオン状態を取得する
     * @return true:サーボオン false:サーボオフ
     */    
    public boolean isServoOn() {
        return unitOperator_.isServoOn();
    }

    /**
     * 制御点位置を保持したまま冗長軸動作（絶対指令）
     * @param [in] type 右or左アーム
     * @param [in] absPos 目標位置
     * @param [in] speed 目標速度
     * @param [in] accel 目標加速度
     */    
    public ReturnID moveCPHoldAbs(RightLeft type, RedundantPos absPos, RedundantSpeed speed, RedundantAccel accel) {
    	ReturnID rtn = new ReturnID();
    	rtn.id = 0;
    	
    	if (!unitOperator_.isServoOn()) {
    		System.out.println("servo is off.");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;
    	}
    	
    	//! 動作モードの選択
    	//! 冗長軸指令モード
    	unitOperator_.selectMode((short) 1, type, false);
    	
    	UnitCommand<IntegratedMotionCommand> command = new UnitCommand<IntegratedMotionCommand>();
    	command.setCommand(IntegratedMotionCommand.MOVE_CP_HOLD_ABS);
    	
    	double[] param = new double[20];
    	
    	//! param[0] type
    	//! param[1]-[7] absPos
    	//! param[8]-[13] speed
    	//! param[14]-[19] accel

    	if (type == RightLeft.RIGHT) {
    		param[0] = 1.0;
    	} else {
    		param[0] = -1.0;
    	}
    	
    	param[1] = absPos.elbow;
    	param[2] = absPos.lumbarRx;
    	param[3] = absPos.lumbarRy;
    	param[4] = absPos.lumbarRz;
    	param[5] = absPos.vehicleX;
    	param[6] = absPos.vehicleY;
    	param[7] = absPos.vehicleTheta;
    	
    	param[8] = speed.elbow;
    	param[9] = speed.lumbarRx;
    	param[10] = speed.lumbarRy;
    	param[11] = speed.lumbarRz;
    	param[12] = speed.vehicleTranslation;
    	param[13] = speed.vehicleRotation;
    	
    	param[14] = accel.elbow;
    	param[15] = accel.lumbarRx;
    	param[16] = accel.lumbarRy;
    	param[17] = accel.lumbarRz;
    	param[18] = accel.vehicleTranslation;
    	param[19] = accel.vehicleRotation;
    	
    	command.setParams(param);
    	
    	if (!commandQ_.put(command)) {
    		System.out.println("cannot push command");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;    		
    	}
    	
    	return rtn;
    }

    /**
     * 制御点位置を保持したまま冗長軸動作（相対指令）
     * @param [in] type 右or左アーム
     * @param [in] relPos 目標位置
     * @param [in] speed 目標速度
     * @param [in] accel 目標加速度
     */    
    public ReturnID moveCPHoldRel(RightLeft type, RedundantPos relPos, RedundantSpeed speed, RedundantAccel accel) {
    	ReturnID rtn = new ReturnID();
    	rtn.id = 0;

    	if (!unitOperator_.isServoOn()) {
    		System.out.println("servo is off.");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;
    	}

    	//! 動作モードの選択
    	//! 冗長軸指令モード
    	unitOperator_.selectMode((short) 1, type, false);
    	
    	UnitCommand<IntegratedMotionCommand> command = new UnitCommand<IntegratedMotionCommand>();
    	command.setCommand(IntegratedMotionCommand.MOVE_CP_HOLD_REL);
    	
    	double[] param = new double[20];
    	
    	//! param[0] type
    	//! param[1]-[7] relPos
    	//! param[8]-[13] speed
    	//! param[14]-[19] accel

    	if (type == RightLeft.RIGHT) {
    		param[0] = 1.0;
    	} else {
    		param[0] = -1.0;
    	}
    	
    	param[1] = relPos.elbow;
    	param[2] = relPos.lumbarRx;
    	param[3] = relPos.lumbarRy;
    	param[4] = relPos.lumbarRz;
    	param[5] = relPos.vehicleX;
    	param[6] = relPos.vehicleY;
    	param[7] = relPos.vehicleTheta;
    	
    	param[8] = speed.elbow;
    	param[9] = speed.lumbarRx;
    	param[10] = speed.lumbarRy;
    	param[11] = speed.lumbarRz;
    	param[12] = speed.vehicleTranslation;
    	param[13] = speed.vehicleRotation;
    	
    	param[14] = accel.elbow;
    	param[15] = accel.lumbarRx;
    	param[16] = accel.lumbarRy;
    	param[17] = accel.lumbarRz;
    	param[18] = accel.vehicleTranslation;
    	param[19] = accel.vehicleRotation;
    	
    	command.setParams(param);
    	
    	if (!commandQ_.put(command)) {
    		System.out.println("cannot push command");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;    		
    	}    	
    	
    	return rtn;
    }

    public ReturnID moveGripper(RightLeft type, int angleRatio) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID moveGripper(RightLeft type, int angleRatio)>"
    	ReturnID rtn = new ReturnID();
    	return rtn;
    }

    public ReturnID moveJogCPHold(RightLeft type, RedundantJogSpeed speed) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID moveJogCPHold(RightLeft type, RedundantJogSpeed speed)>"
    	ReturnID rtn = new ReturnID();
    	return rtn;
    }

    public ReturnID moveJogLinear(RightLeft type, CommandFrameType frameType, CartesianJogSpeed speed, boolean isHoldOtherArm) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID moveJogLinear(RightLeft type, CommandFrameType frameType, CartesianJogSpeed speed, boolean isHoldOtherArm)>"
    	ReturnID rtn = new ReturnID();
    	return rtn;
    }

/*    
    public ReturnID moveLinearCartesianAbs(RightLeft type, double absHgMat, CartesianSpeed speed, CartesianAccel accel, boolean isHoldOtherArm) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID moveLinearCartesianAbs(RightLeft type, HgMatrix absHgMat, CartesianSpeed speed, CartesianAccel accel, boolean isHoldOtherArm)>"
        return 0;
    }

    public ReturnID moveLinearCartesianRel(RightLeft type, CommandFrameType frameType, double relHgMat, CartesianSpeed speed, CartesianAccel accel, boolean isHoldOtherArm) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID moveLinearCartesianRel(RightLeft type, CommandFrameType frameType, HgMatrix relHgMat, CartesianSpeed speed, CartesianAccel accel, boolean isHoldOtherArm)>"
        return 0;
    }

    public ReturnID movePTPCartesianAbs(RightLeft type, double absHgMat, double time, boolean isHoldOtherArm) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID movePTPCartesianAbs(RightLeft type, HgMatrix absHgMat, double time, boolean isHoldOtherArm)>"
        return 0;
    }

    public ReturnID movePTPCartesianRel(RightLeft type, double relHgMat, double time, boolean isHoldOtherArm) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID movePTPCartesianRel(RightLeft type, HgMatrix relHgMat, double time, boolean isHoldOtherArm)>"
        return 0;
    }
*/
    
    /**
     * リンク補間（絶対位置指令）
     * @param [in] type ユニットの種別
     * @param [in] absPos 目標位置
     * @param [in] speed 目標速度
     * @param [in] accel 目標加速度
     */        
    public ReturnID moveUnitAbs(UnitType type, double[] absPos, double[] speed, double[] accel) {
    	ReturnID rtn = new ReturnID(0,"");
    	
    	if (!unitOperator_.isServoOn()) {
    		System.out.println("servo is off.");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;
    	}
    	
    	UnitCommand<IntegratedMotionCommand> command = new UnitCommand<IntegratedMotionCommand>();
    	command.setCommand(IntegratedMotionCommand.MOVE_UNIT_ABS);
    	
    	double[] param = new double[18];
    	
    	if (type == UnitType.RIGHT_ARM) {
    		param[0] = 1.0;
    	} else if (type == UnitType.LEFT_ARM){
    		param[0] = 2.0;
    	} else if (type == UnitType.LUMBER){
    		param[0] = 3.0;
    	} else {
    		param[0] = 4.0;
    	}
    	
    	param[1] = absPos[0];
    	param[2] = absPos[1];
    	param[3] = absPos[2];
    	param[4] = absPos[3];
    	param[5] = absPos[4];
    	param[6] = absPos[5];
    	param[7] = absPos[6];

    	param[8] = speed[0];

    	param[9] = accel[0];
    	
    	command.setParams(param);
    	
    	if (!commandQ_.put(command)) {
    		System.out.println("cannot push command");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;    		
    	}
    	
    	return rtn;
    }

    /**
     * リンク補間（相対位置指令）
     * @param [in] type ユニットの種別
     * @param [in] relPos 目標位置
     * @param [in] speed 目標速度
     * @param [in] accel 目標加速度
     */            
    public ReturnID moveUnitRel(UnitType type, double[] relPos, double[] speed, double[] accel) {
    	ReturnID rtn = new ReturnID(0,"");
    	
    	if (!unitOperator_.isServoOn()) {
    		System.out.println("servo is off.");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;
    	}
    	
    	UnitCommand<IntegratedMotionCommand> command = new UnitCommand<IntegratedMotionCommand>();
    	command.setCommand(IntegratedMotionCommand.MOVE_UNIT_REL);
    	
    	double[] param = new double[18];
    	
    	if (type == UnitType.RIGHT_ARM) {
    		param[0] = 1.0;
    	} else if (type == UnitType.LEFT_ARM){
    		param[0] = 2.0;
    	} else if (type == UnitType.LUMBER){
    		param[0] = 3.0;
    	} else {
    		param[0] = 4.0;
    	}
    	
    	param[1] = relPos[0];
    	param[2] = relPos[1];
    	param[3] = relPos[2];
    	param[4] = relPos[3];
    	param[5] = relPos[4];
    	param[6] = relPos[5];
    	param[7] = relPos[6];

    	param[8] = speed[0];

    	param[9] = accel[0];
    	
    	command.setParams(param);
    	
    	if (!commandQ_.put(command)) {
    		System.out.println("cannot push command");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;    		
    	}
    	
    	return rtn;

    }

    public ReturnID openGripper(RightLeft type) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID openGripper(RightLeft type)>"
    	ReturnID rtn = new ReturnID();
    	return rtn;
    }

    /**
     * 一時停止
     */    
    public ReturnID pause() {
    	ReturnID rtn = new ReturnID();
    	rtn = unitOperator_.pause();
    	return rtn;
    }

    public ReturnID resetOriginalFrame() {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID resetOriginalFrame()>"
    	ReturnID rtn = new ReturnID();
    	return rtn;
    }

    /**
     * 動作再開
     */    
    public ReturnID resume() {
    	ReturnID rtn = new ReturnID();
    	rtn = unitOperator_.resume();
    	return rtn;
    }

    /**
     * 冗長軸のマスクおよび動作可能範囲を設定する
     * @param redundancy 冗長軸の指定
     * @param limit 冗長軸の動作可能範囲
     */    
    public ReturnID selectRedundantAxes(RedundantAxesMask redundancy, RedundantLimit limit) {
    	ReturnID rtn = new ReturnID();
    	rtn = unitOperator_.selectRedundantAxes(redundancy);
    	rtn = unitOperator_.selectRedundantLimit(limit);
    	return rtn;
    }

    /**
     * サーボオフ
     * 
     */    
    public ReturnID servoOff() {
    	ReturnID rtn = new ReturnID(0,"");
    	rtn = unitOperator_.servoOff();
    	return rtn;
    }

    /**
     * サーボオン
     * 
     */    
    public ReturnID servoOn() {
    	ReturnID rtn = new ReturnID(0,"");
    	rtn = unitOperator_.servoOn();
    	return rtn;
    }

/*    
    public ReturnID setControlPointOffset(RightLeft type, double offset) {
    }
*/
    public ReturnID setJogTimeout(int time) {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID setJogTimeout(int time)>"
    	ReturnID rtn = new ReturnID();
    	return rtn;
    }

    public ReturnID stop() {
        // Please insert your code here and remove the following warning pragma
        // TODO "Code missing in function <ReturnID stop()>"
    	ReturnID rtn = new ReturnID();
    	return rtn;
    }

    /**
     * 原点フレームに基づく直線補間（絶対指令）
     * @param type 右or左アーム
     * @param absHgMat 目標位置を指定する同次変換行列
     * @param speed 動作速度
     * @param accel 動作加速度
     * @param inHoldOtherArm 反対のアーム維持の有無
     */
	public ReturnID moveLinearCartesianAbs(RightLeft type, double[][] absHgMat,
			CartesianSpeed speed, CartesianAccel accel, boolean isHoldOtherArm) {
    	ReturnID rtn = new ReturnID(0,"");
    	
    	if (!unitOperator_.isServoOn()) {
    		System.out.println("servo is off.");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;
    	}
    	
    	//! 動作モードの選択
    	//! CPモーションモード
    	unitOperator_.selectMode((short) 0, type, isHoldOtherArm);
    	
		jp.co.yaskawa.rtc.integratedMotion.controller.RedundantAxesMask mask = 
			new jp.co.yaskawa.rtc.integratedMotion.controller.RedundantAxesMask();
		mask.elbow = true;		
    	unitOperator_.selectRedundantAxes(mask);
    	
    	UnitCommand<IntegratedMotionCommand> command = new UnitCommand<IntegratedMotionCommand>();
    	command.setCommand(IntegratedMotionCommand.MOVE_LINEAR_CAR_ABS);
    	
    	double[] param = new double[18];
    	
    	//! param[0] type
    	//! param[1]-[12] absHgMat
    	//! param[13][14] speed
    	//! param[15][16] accel
    	//! param[17] isHoldOtherArm

    	if (type == RightLeft.RIGHT) {
    		param[0] = 1.0;
    	} else {
    		param[0] = -1.0;
    	}
    	
    	param[1] = absHgMat[0][0];
    	param[2] = absHgMat[0][1];
    	param[3] = absHgMat[0][2];
    	param[4] = absHgMat[0][3];
    	param[5] = absHgMat[1][0];
    	param[6] = absHgMat[1][1];
    	param[7] = absHgMat[1][2];
    	param[8] = absHgMat[1][3];
    	param[9] = absHgMat[2][0];
    	param[10] = absHgMat[2][1];
    	param[11] = absHgMat[2][2];
    	param[12] = absHgMat[2][3];

    	param[13] = speed.translation;
    	param[14] = speed.rotation;

    	param[15] = accel.translation;
    	param[16] = accel.rotation;
    	
    	if (isHoldOtherArm) {
    		param[17] = 1.0;
    	} else {
    		param[17] = -1.0;
    	}
    	
    	command.setParams(param);
    	
    	if (!commandQ_.put(command)) {
    		System.out.println("cannot push command");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;    		
    	}
    	
    	return rtn;
    	    	
	}

    /**
     * 原点フレームに基づく直線補間（相対指令）
     * @param type 右or左アーム
     * @param absHgMat 目標位置を指定する同次変換行列
     * @param speed 動作速度
     * @param accel 動作加速度
     * @param inHoldOtherArm 反対のアーム維持の有無
     */    	
	public ReturnID moveLinearCartesianRel(RightLeft type,
			CommandFrameType frameType, double[][] relHgMat,
			CartesianSpeed speed, CartesianAccel accel, boolean isHoldOtherArm) {
    	ReturnID rtn = new ReturnID(0,"");
    	
    	if (!unitOperator_.isServoOn()) {
    		System.out.println("servo is off.");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;
    	}
    	
    	//! 動作モードの選択
    	//! CPモーションモード
    	unitOperator_.selectMode((short) 0, type, isHoldOtherArm);    	    	
    	
		jp.co.yaskawa.rtc.integratedMotion.controller.RedundantAxesMask mask = 
			new jp.co.yaskawa.rtc.integratedMotion.controller.RedundantAxesMask();
		mask.elbow = true;		
    	unitOperator_.selectRedundantAxes(mask);
    	
    	
    	UnitCommand<IntegratedMotionCommand> command = new UnitCommand<IntegratedMotionCommand>();
    	command.setCommand(IntegratedMotionCommand.MOVE_LINEAR_CAR_REL);
    	
    	double[] param = new double[19];
    	
    	//! param[0] type
    	//! param[1]-[12] relHgMat
    	//! param[13][14] speed
    	//! param[15][16] accel
    	//! param[17] isHoldOtherArm
    	//! param[18] frameType

    	if (type == RightLeft.RIGHT) {
    		param[0] = 1.0;
    	} else {
    		param[0] = -1.0;
    	}
    	
    	param[1] = relHgMat[0][0];
    	param[2] = relHgMat[0][1];
    	param[3] = relHgMat[0][2];
    	param[4] = relHgMat[0][3];
    	param[5] = relHgMat[1][0];
    	param[6] = relHgMat[1][1];
    	param[7] = relHgMat[1][2];
    	param[8] = relHgMat[1][3];
    	param[9] = relHgMat[2][0];
    	param[10] = relHgMat[2][1];
    	param[11] = relHgMat[2][2];
    	param[12] = relHgMat[2][3];

    	param[13] = speed.translation;
    	param[14] = speed.rotation;

    	param[15] = accel.translation;
    	param[16] = accel.rotation;
    	
    	if (isHoldOtherArm) {
    		param[17] = 1.0;
    	} else {
    		param[17] = -1.0;
    	}
    	
    	if (frameType == CommandFrameType.BASE_FRAME) {
    		param[18] = 1.0;
    	} else {
    		param[18] = -1.0;
    	}
    	
    	command.setParams(param);
    	
    	if (!commandQ_.put(command)) {
    		System.out.println("cannot push command");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;    		
    	}
    	
    	return rtn;
    	
	}

    /**
     *  関節空間において、目標位置を絶対直交座標指定により、直線補間を動作する
     *  @param [in] type 右or左アーム
     *  @param [in] absHgMat 目標位置（同次変換行列 3x4）
     *  @param [in] time 移動時間[s]
     *  @param [in] isHoldOtherArm 他アームの制御
     */
	public ReturnID movePTPCartesianAbs(RightLeft type, double[][] absHgMat,
			double time, boolean isHoldOtherArm) {
    	
    	ReturnID rtn = new ReturnID(0,"");
    	
    	if (!unitOperator_.isServoOn()) {
    		System.out.println("servo is off.");
    		rtn.id = -1;
    		//! TODO return error code
    		return rtn;
    	}
    	
        double[] commandPosture = new double[24];
    	double[] currentPostureOther_;
    	    	
    	if (type == RightLeft.RIGHT) {
    		
    		commandPosture[0] = absHgMat[0][3];
    		commandPosture[1] = absHgMat[1][3];
    		commandPosture[2] = absHgMat[2][3];
    		commandPosture[3] = absHgMat[0][0];
    		commandPosture[4] = absHgMat[0][1];
    		commandPosture[5] = absHgMat[0][2];
    		commandPosture[6] = absHgMat[1][0];
    		commandPosture[7] = absHgMat[1][1];
    		commandPosture[8] = absHgMat[1][2];
    		commandPosture[9] = absHgMat[2][0];
    		commandPosture[10] = absHgMat[2][1];
    		commandPosture[11] = absHgMat[2][2];
    		
			currentPostureOther_ = imInterpolator_.getCartesianFeedbackPos(RightLeft.LEFT, FrameType.ORIGINAL_FRAME);
			
			for(int i=0; i<12; i++) {
				commandPosture[i+12] = currentPostureOther_[i];  
			}
			
    	} else {
			currentPostureOther_ = imInterpolator_.getCartesianFeedbackPos(RightLeft.RIGHT, FrameType.ORIGINAL_FRAME);
			for(int i=0; i<12; i++) {
				commandPosture[i] = currentPostureOther_[i];  
			}
			
    		commandPosture[12] = absHgMat[0][3];
    		commandPosture[13] = absHgMat[1][3];
    		commandPosture[14] = absHgMat[2][3];
    		commandPosture[15] = absHgMat[0][0];
    		commandPosture[16] = absHgMat[0][1];
    		commandPosture[17] = absHgMat[0][2];
    		commandPosture[18] = absHgMat[1][0];
    		commandPosture[19] = absHgMat[1][1];
    		commandPosture[20] = absHgMat[1][2];
    		commandPosture[21] = absHgMat[2][0];
    		commandPosture[22] = absHgMat[2][1];
    		commandPosture[23] = absHgMat[2][2];
			
    	}
    	//! 目標位置を冗長性利用モジュールに出力
    	unitOperator_.setCartesianCommand(commandPosture);
    
    	
    	
    	//! 逆演算結果を取得
    	double[] ikinema = new double[24]; 
    	unitOperator_.getIKinematicsResult(ikinema);
    	
    	double[] rightArmPos = new double[7];
    	for(int i=0; i<7; i++){
        	rightArmPos[i] = ikinema[6+i];
    	}
    	double[] spd = {10.0,10.0};
    	double[] acc = {10.0,10.0};
    	
    	moveUnitAbs(UnitType.RIGHT_ARM, rightArmPos, spd, acc);
    	
    	return rtn;
    	
    	
	}

	public ReturnID movePTPCartesianRel(RightLeft type, double[][] relHgMat,
			double time, boolean isHoldOtherArm) {
		// TODO Auto-generated method stub
    	ReturnID rtn = new ReturnID();
    	return rtn;
	}

    /**
     * 制御点のオフセット量を指定する
     * @param [in] type 右or左アーム
     * @param [in] offset オフセット量（同次変換行列 3x4）
     */    	
	public ReturnID setControlPointOffset(RightLeft type, double[][] offset) {
    	ReturnID rtn = new ReturnID();
    	rtn = unitOperator_.setControlPointOffset(type, offset);
    	return rtn;
	}

//  End of example implementational code
}
