package jp.co.yaskawa.rtc.integratedMotion;
// -*- Java -*-
/*!
 * @file  IntegratedMotionImpl.java
 * @brief ${rtcParam.description}
 * @date  $Date$
 *
 * $Id$
 */

import java.io.BufferedWriter;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Arrays;
import java.util.Properties;
import java.util.InvalidPropertiesFormatException;

import jp.co.yaskawa.rtc.integratedMotion.controller.AlarmSeqHolder;
import jp.co.yaskawa.rtc.integratedMotion.controller.CarPosWithElbowHolder;
import jp.co.yaskawa.rtc.integratedMotion.controller.FrameType;
import jp.co.yaskawa.rtc.integratedMotion.controller.RedundantAxesMask;
import jp.co.yaskawa.rtc.integratedMotion.controller.RedundantLimit;
import jp.co.yaskawa.rtc.integratedMotion.controller.ReturnID;
import jp.co.yaskawa.rtc.integratedMotion.controller.RightLeft;
import jp.co.yaskawa.rtc.integratedMotion.controller.UnitType;
import jp.co.yaskawa.rtc.integratedMotion.controller.LimitValue;
import jp.co.yaskawa.rtc.integratedMotion.module.UnitOperation;

import jp.co.yaskawa.rtc.integratedMotion.armService.ManipulatorCommonInterface_Common;
import jp.co.yaskawa.rtc.integratedMotion.module.IntegratedMotionInterpolator;
import jp.co.yaskawa.rtc.integratedMotion.RTC.RETURN_ID;
import jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundancyConfig;

import jp.co.yaskawa.rtc.integratedMotion.RTC.TimedDoubleSeq;
import jp.co.yaskawa.rtc.integratedMotion.RTC.Time;
import jp.co.yaskawa.rtc.integratedMotion.RTC.TimedJointPos;
import jp.go.aist.rtm.RTC.DataFlowComponentBase;
import jp.go.aist.rtm.RTC.Manager;
import jp.go.aist.rtm.RTC.port.ConnectorDataListenerT;
import jp.go.aist.rtm.RTC.port.InPort;
import jp.go.aist.rtm.RTC.port.OutPort;
import jp.go.aist.rtm.RTC.util.DataRef;
import jp.go.aist.rtm.RTC.port.CorbaConsumer;
import jp.go.aist.rtm.RTC.port.CorbaPort;
import jp.go.aist.rtm.RTC.port.ConnectorBase.ConnectorInfo;

import org.omg.CORBA.IntHolder;
import org.omg.PortableServer.POAPackage.ObjectNotActive;
import org.omg.PortableServer.POAPackage.ServantAlreadyActive;
import org.omg.PortableServer.POAPackage.WrongPolicy;
import jp.go.aist.rtm.RTC.util.StringHolder;
import RTC.ReturnCode_t;

import jp.co.yaskawa.rtc.integratedMotion.lumbarService.LumbarUnit;
import jp.co.yaskawa.rtc.integratedMotion.vehicleService.VehicleService;

/*!
 * @class IntegratedMotionImpl
 * @brief ${rtcParam.description}
 *
 */
public class IntegratedMotionImpl extends DataFlowComponentBase implements UnitOperation{

    private RedundantAxesMask redundantAxesMask_ = null; //!< 冗長軸の指定情報
    private RedundantLimit redundantLimit_ = null; //!< 冗長軸のリミット値情報
    private boolean sendStatus_ = true; 
    Properties prop = new Properties();
    int segmentPeriod = 0; //!< セグメント周期[ms]
    double[] cartesianFeedback_;
    double[] kinematicsResult_;
    double[] redundantFeedback_;
    double[] armR_jointFeedback_;
    double[] armL_jointFeedback_;
    double[] lumbar_jointFeedback_;
    double[] vehicle_Feedback_;
    
    boolean isConnectArmR_;
    boolean isConnectArmL_;
    boolean isConnectLumbar_;
    boolean isConnectVehicle_;
    
    boolean isServoOn_;
    
    File logCarPos;
    BufferedWriter outCarPos;
	
    //! add 2011.02.03
    int robotType_ = 1;          //!< 0:PA10, 1:SmartPal, 2:RH
    
    int armRJointNum_ = 0;       //!< 右アームの軸数
    int armLJointNum_ = 0;       //!< 左アームの軸数
    int handRJointNum_ = 0;      //!< 右ハンドの軸数
    int handLJointNum_ = 0;      //!< 左ハンドの軸数
    int lumbar_axes_ = 0;        //!< 腰軸数
    int vehicle_axes_ = 0;        //!< 移動台車軸数　 
        
    
  /*!
   * @brief constructor
   * @param manager Maneger Object
   */
	public IntegratedMotionImpl(Manager manager) {  
        super(manager);
        // <rtc-template block="initializer">
        m_arm_cartesian_fb_in_val = new TimedDoubleSeq();
        m_arm_cartesian_fb_in = new DataRef<TimedDoubleSeq>(m_arm_cartesian_fb_in_val);
        m_arm_cartesian_fb_inIn = new InPort<TimedDoubleSeq>("arm_cartesian_fb_in", m_arm_cartesian_fb_in);
        m_iKinema_result_in_val = new TimedDoubleSeq();
        m_iKinema_result_in = new DataRef<TimedDoubleSeq>(m_iKinema_result_in_val);
        m_iKinema_result_inIn = new InPort<TimedDoubleSeq>("iKinema_result_in", m_iKinema_result_in);
        m_redundant_fb_in_val = new TimedDoubleSeq();
        m_redundant_fb_in = new DataRef<TimedDoubleSeq>(m_redundant_fb_in_val);
        m_redundant_fb_inIn = new InPort<TimedDoubleSeq>("redundant_fb_in", m_redundant_fb_in);
        m_joint_fb_in_val = new TimedDoubleSeq();
        m_joint_fb_in = new DataRef<TimedDoubleSeq>(m_joint_fb_in_val);
        m_joint_fb_inIn = new InPort<TimedDoubleSeq>("joint_fb_in", m_joint_fb_in);
        m_status_in_val = new TimedDoubleSeq();
        m_status_in = new DataRef<TimedDoubleSeq>(m_status_in_val);
        m_status_inIn = new InPort<TimedDoubleSeq>("status_in", m_status_in);
        m_armR_joint_cmd_out_val = new TimedJointPos();
        m_armR_joint_cmd_out = new DataRef<TimedJointPos>(m_armR_joint_cmd_out_val);
        m_armR_joint_cmd_outOut = new OutPort<TimedJointPos>("armR_joint_cmd_out", m_armR_joint_cmd_out);
        m_armL_joint_cmd_out_val = new TimedJointPos();
        m_armL_joint_cmd_out = new DataRef<TimedJointPos>(m_armL_joint_cmd_out_val);
        m_armL_joint_cmd_outOut = new OutPort<TimedJointPos>("armL_joint_cmd_out", m_armL_joint_cmd_out);
        m_lumbar_cmd_out_val = new TimedJointPos();
        m_lumbar_cmd_out = new DataRef<TimedJointPos>(m_lumbar_cmd_out_val);
        m_lumbar_cmd_outOut = new OutPort<TimedJointPos>("lumbar_cmd_out", m_lumbar_cmd_out);
        m_vehicle_cmd_out_val = new TimedDoubleSeq();
        m_vehicle_cmd_out = new DataRef<TimedDoubleSeq>(m_vehicle_cmd_out_val);
        m_vehicle_cmd_outOut = new OutPort<TimedDoubleSeq>("vehicle_cmd_out", m_vehicle_cmd_out);
        m_arm_cartesian_cmd_out_val = new TimedDoubleSeq();
        m_arm_cartesian_cmd_out = new DataRef<TimedDoubleSeq>(m_arm_cartesian_cmd_out_val);
        m_arm_cartesian_cmd_outOut = new OutPort<TimedDoubleSeq>("arm_cartesian_cmd_out", m_arm_cartesian_cmd_out);
        m_redundant_cmd_out_val = new TimedDoubleSeq();
        m_redundant_cmd_out = new DataRef<TimedDoubleSeq>(m_redundant_cmd_out_val);
        m_redundant_cmd_outOut = new OutPort<TimedDoubleSeq>("redundant_cmd_out", m_redundant_cmd_out);
        m_controllerPort = new CorbaPort("controller");
        m_toRedundancyPort = new CorbaPort("toRedundancy");
        m_armL_servicePort = new CorbaPort("armL_service");
        m_armR_servicePort = new CorbaPort("armR_service");
        m_lumbar_servicePort = new CorbaPort("lumbar_service");
        m_vehicle_servicePort = new CorbaPort("vehicle_service");
        // </rtc-template>
        redundantAxesMask_ = new RedundantAxesMask();
        redundantAxesMask_.allEnable();
        
        redundantLimit_ = new RedundantLimit();
        redundantLimit_.rightElbow = new LimitValue();
        redundantLimit_.leftElbow = new LimitValue();
        redundantLimit_.lumbarRx = new LimitValue();
        redundantLimit_.lumbarRy = new LimitValue();
        redundantLimit_.lumbarRz = new LimitValue();
        redundantLimit_.vehicleX = new LimitValue();
        redundantLimit_.vehicleY = new LimitValue();
        redundantLimit_.vehicleTheta = new LimitValue();
        redundantLimit_.rightElbow.upper = 90.0;
        redundantLimit_.rightElbow.lower = -90.0;
        redundantLimit_.leftElbow.upper = 90.0;
        redundantLimit_.leftElbow.lower = -90.0;
        redundantLimit_.lumbarRx.upper = 90.0;
        redundantLimit_.lumbarRx.lower = 0.0;
        redundantLimit_.lumbarRy.upper = 90.0;
        redundantLimit_.lumbarRy.lower = 0.0;
        redundantLimit_.lumbarRz.upper = 90.0;
        redundantLimit_.lumbarRz.lower = 0.0;
        redundantLimit_.vehicleX.upper = 0.0;
        redundantLimit_.vehicleX.lower = 0.0;
        redundantLimit_.vehicleY.upper = 0.0;
        redundantLimit_.vehicleY.lower = 0.0;
        redundantLimit_.vehicleTheta.upper = 90.0;
        redundantLimit_.vehicleTheta.lower = -90.0;

        isConnectArmR_ = false;
        isConnectArmL_ = false;
        isConnectLumbar_ = false;
        isConnectVehicle_ = false;
        
        isServoOn_ = false;

        m_armR_joint_cmd_out.v.pos = new double[8];
        m_armL_joint_cmd_out.v.pos = new double[8];
        m_lumbar_cmd_out.v.pos = new double[3];
        m_vehicle_cmd_out.v.data = new double[3];
        m_arm_cartesian_cmd_out.v.data = new double[24];
        m_redundant_cmd_out.v.data = new double[8];
        
        m_armR_joint_cmd_out.v.tm = new Time(0,0);
        m_armL_joint_cmd_out.v.tm = new Time(0,0);
        m_lumbar_cmd_out.v.tm = new Time(0,0);
        m_vehicle_cmd_out.v.tm = new Time(0,0);
        m_arm_cartesian_cmd_out.v.tm = new Time(0,0);
        m_redundant_cmd_out.v.tm = new Time(0,0);
/*        
        m_iKinema_result_inIn.addConnectorDataListener(
        		ConnectorDataListenerType.ON_BUFFER_WRITE,
        		new DataListener("ON_BUFFER_WRITE")
        		);
*/
    }

    /*!
     *
     * The initialize action (on CREATED->ALIVE transition)
     * formaer rtc_init_entry() 
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
    @Override
    protected ReturnCode_t onInitialize() {
        // Registration: InPort/OutPort/Service
        // <rtc-template block="registration">
        // Set InPort buffers
        addInPort("arm_cartesian_fb_in", m_arm_cartesian_fb_inIn);
        addInPort("iKinema_result_in", m_iKinema_result_inIn);
        addInPort("redundant_fb_in", m_redundant_fb_inIn);
        addInPort("joint_fb_in", m_joint_fb_inIn);
        addInPort("status_in", m_status_inIn);
        
        // Set OutPort buffer
        addOutPort("armR_joint_cmd_out", m_armR_joint_cmd_outOut);
        addOutPort("armL_joint_cmd_out", m_armL_joint_cmd_outOut);
        addOutPort("lumbar_cmd_out", m_lumbar_cmd_outOut);
        addOutPort("vehicle_cmd_out", m_vehicle_cmd_outOut);
        addOutPort("arm_cartesian_cmd_out", m_arm_cartesian_cmd_outOut);
        addOutPort("redundant_cmd_out", m_redundant_cmd_outOut);
        
        // Set service provider to Ports
        try {
        	m_controllerPort.registerProvider("toClient", "IntegratedMotionInterface", m_toClient);
        } catch (ServantAlreadyActive e) {
            e.printStackTrace();
        } catch (WrongPolicy e) {
            e.printStackTrace();
        } catch (ObjectNotActive e) {
            e.printStackTrace();
        }
        
        // Set service consumers to Ports
        m_toRedundancyPort.registerConsumer("toRedundancy", "RedundancyConfig", m_redundancyConfigBase);
        m_armL_servicePort.registerConsumer("leftarmService_Common", "ManipulatorCommonInterface_Common", m_armLBase);
        m_armR_servicePort.registerConsumer("rightarmService_Common", "ManipulatorCommonInterface_Common", m_armRBase);
        m_lumbar_servicePort.registerConsumer("LumbarService", "LumbarUnit", m_lumbarBase);
        m_vehicle_servicePort.registerConsumer("vehicleService", "VehicleService", m_vehicleBase);
        
        // Set CORBA Service Ports
        addPort(m_controllerPort);
        addPort(m_toRedundancyPort);
        addPort(m_armL_servicePort);
        addPort(m_armR_servicePort);
        addPort(m_lumbar_servicePort);
        addPort(m_vehicle_servicePort);
        // </rtc-template>
        bindParameter("configFile", m_configFile, "default.conf");
        return super.onInitialize();
    }

    /***
     *
     * The finalize action (on ALIVE->END transition)
     * formaer rtc_exiting_entry()
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onFinalize() {
//        return super.onFinalize();
//    }

    /***
     *
     * The startup action when ExecutionContext startup
     * former rtc_starting_entry()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onStartup(int ec_id) {
//        return super.onStartup(ec_id);
//    }

    /***
     *
     * The shutdown action when ExecutionContext stop
     * former rtc_stopping_entry()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onShutdown(int ec_id) {
//        return super.onShutdown(ec_id);
//    }

    /***
     *
     * The activated action (Active state entry action)
     * former rtc_active_entry()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
    @Override
    protected ReturnCode_t onActivated(int ec_id) {
    	System.out.println("onActivated()");
    	
    	try {
    		prop.loadFromXML(new FileInputStream(m_configFile.value));
    	} catch(InvalidPropertiesFormatException e) {
    		System.err.println(e);
    		return ReturnCode_t.RTC_ERROR;
    	} catch(IOException e) {
    		System.err.println(e);
    		return ReturnCode_t.RTC_ERROR;
    	}

    	try {
    		double d = Double.parseDouble(prop.getProperty("segmentPeriod"));
    		segmentPeriod = (int)Math.floor(d * 1000); // セグメント周期．1ミリ秒以下は切り捨て
//    		nJoint = Integer.parseInt(prop.getProperty("nJoint")); // 関節数
//    		nPosture = Integer.parseInt(prop.getProperty("nPosture")); // 位置姿勢データ数
    		
    		// 関節オフセット取得
//    		String keyBase ="offset.";
//    		String key;
//    		offset = new double[nJoint];
//    		for (int i = 0; i < nJoint; i++) {
//    			key = keyBase + i;
//    			offset[i] = Double.parseDouble(prop.getProperty(key));
//    		}
    		
    	} catch(NumberFormatException e) {
    		System.err.println(e);
    		return ReturnCode_t.RTC_ERROR;
    	}    	
    	
    	imInterpolator.setSegmentPeriod((double)segmentPeriod/1000);
   
        if (m_armR == null) {
    		m_armR = m_armRBase._ptr();
    	}
        
        if (m_armL == null) {
    		m_armL = m_armLBase._ptr();
    	}
        
        if (m_lumbar == null) {
        	m_lumbar = m_lumbarBase._ptr();
    	}

        if (m_vehicle == null) {
        	m_vehicle = m_vehicleBase._ptr();
    	}

        if (m_redundancyConfig == null) {
    		m_redundancyConfig = m_redundancyConfigBase._ptr();
    	}


    	switch (robotType_) {
    	case 0:
    		System.out.println("PA10:");
    		isConnectArmR_ = true;
    		isConnectArmL_ = false;
    		isConnectLumbar_ = false;
    		isConnectVehicle_ = false;
    		
    	    armRJointNum_ = 7;
    	    handRJointNum_ = 1;
    	    break;

    	case 1:
    		System.out.println("SmartPal:");
    		isConnectArmR_ = true;
    		isConnectArmL_ = true;
    		isConnectLumbar_ = true;
    		isConnectVehicle_ = true;
    		
            armRJointNum_ = 7;       //!< 右アームの軸数
            armLJointNum_ = 7;       //!< 左アームの軸数
            handRJointNum_ = 1;      //!< 右ハンドの軸数
            handLJointNum_ = 1;      //!< 左ハンドの軸数
//          lumbar_axes_ = 2;        //!< 腰軸ry有り
//          vehicle_axes_ = 7;       //!< 移動軸x,y,theta有り
//          lumbar_axes_ = 1;        //!< 腰軸ry有り
            lumbar_axes_ = 3;
            vehicle_axes_ = 3;       //!< 移動軸x,y,theta有り
            break;

    	case 2:
    		System.out.println("ReferenceHard:");
    		isConnectArmR_ = true;
    		isConnectArmL_ = false;
    		isConnectLumbar_ = false;
    		isConnectVehicle_ = true;
    		
            armRJointNum_ = 6;       //!< 右アームの軸数
            handRJointNum_ = 1;      //!< 右ハンドの軸数
            vehicle_axes_ = 3;       //!< 移動軸x,y有り
            break;
    	
    	default:
    		System.out.println("unknown robot type:");            	
            break;
    	}
   	
    	boolean[] isUnits = new boolean[4]; 
    	int[] axes_info = new int[6];
    		
    	isUnits[0] = isConnectArmR_;
    	isUnits[1] = isConnectArmL_;
    	isUnits[2] = isConnectLumbar_;
    	isUnits[3] = isConnectVehicle_;
    	
    	axes_info[0] = armRJointNum_;
    	axes_info[1] = armLJointNum_;
    	axes_info[2] = handRJointNum_;
    	axes_info[3] = handLJointNum_;
    	axes_info[4] = lumbar_axes_;
    	axes_info[5] = vehicle_axes_;

    	imInterpolator.setConnectUnitsInfo(isUnits, axes_info);
    	
    	cartesianFeedback_ = new double[24]; //!< x,y,z,3x3Matrix * 2 (Right & Left)
    	for (int i=0; i<24; i++) {
    		cartesianFeedback_[i] = 0.0;
    	}
    	
    	redundantFeedback_ = new double[8]; //!< vehicle(x,y,theta),lumbar(rx,ry,rz),right elbow, left elbow
    	for (int i=0; i<8; i++) {
    		redundantFeedback_[i] = 0.0;
    	}
    	
    	kinematicsResult_ = new double[24]; //!< vehicle(x,y,theta),lumbar(rx,ry,rz),right arm(J1-J7),left arm(J1-J7),head(1,2,3)
    	for (int i=0; i<24; i++) {
    		kinematicsResult_[i] = 0.0;
    	}
    	
        armR_jointFeedback_ = new double[8]; //!< J1-J7, Gripper
    	for (int i=0; i<armR_jointFeedback_.length; i++) {
    		armR_jointFeedback_[i] = 0.0;
    	}

    	armL_jointFeedback_ = new double[8]; //!< J1-J7, Gripper
    	for (int i=0; i<armL_jointFeedback_.length; i++) {
    		armL_jointFeedback_[i] = 0.0;
    	}
    	
    	lumbar_jointFeedback_ = new double[3]; //!< rx,ry,rz
    	for (int i=0; i<lumbar_jointFeedback_.length; i++) {
    		lumbar_jointFeedback_[i] = 0.0;
    	}

    	vehicle_Feedback_ = new double[3]; //!< x,y,theta
    	for (int i=0; i<vehicle_Feedback_.length; i++) {
    		vehicle_Feedback_[i] = 0.0;
    	}
    	    	    	
    	imInterpolator.getQ().clear();
    	imInterpolator.getQ().setReadProtect(false);
    	imInterpolator.getQ().setWriteProtect(false);
    	imInterpolator.setPower(true);
    	
/*    	
		try {
	        logCarPos = new File("logCarPos.dat");    					
			outCarPos = new BufferedWriter(new FileWriter(logCarPos));
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
*/
        return super.onActivated(ec_id);
    }

    /***
     *
     * The deactivated action (Active state exit action)
     * former rtc_active_exit()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
    @Override
    protected ReturnCode_t onDeactivated(int ec_id) {
    	System.out.println("onDeactivated()");
/*    	
    	try {
			outCarPos.close();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
*/    	
        return super.onDeactivated(ec_id);
    }

    /***
     *
     * The execution action that is invoked periodically
     * former rtc_active_do()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
    @Override
    protected ReturnCode_t onExecute(int ec_id) {
	    	
    	if(m_joint_fb_inIn.isNew()) {
        	m_joint_fb_inIn.read();

        	if(m_joint_fb_in.v.data != null) {
        		for(int i=0; i<armR_jointFeedback_.length; i++) {
        			armR_jointFeedback_[i] = m_joint_fb_in.v.data[i+6];
        			armL_jointFeedback_[i] = m_joint_fb_in.v.data[i+14];
        		}

        		for(int j=0; j<vehicle_Feedback_.length; j++) {
            		vehicle_Feedback_[j] = m_joint_fb_in.v.data[j];
            		lumbar_jointFeedback_[j] = m_joint_fb_in.v.data[j+3];        		
        		}	
        	}    		
    	}
    	
    	if(m_arm_cartesian_fb_inIn.isNew()) {
        	m_arm_cartesian_fb_inIn.read();
        	
        	if(m_arm_cartesian_fb_in.v.data != null) {
        		for(int i=0; i<cartesianFeedback_.length; i++) {
        			cartesianFeedback_[i] = m_arm_cartesian_fb_in.v.data[i];	
        		}
        	}
    	}
    	
    	if(m_redundant_fb_inIn.isNew()) {
        	m_redundant_fb_inIn.read();

        	if(m_redundant_fb_in.v.data != null) {
        		for(int i=0; i<redundantFeedback_.length; i++) {
        			redundantFeedback_[i] = m_redundant_fb_in.v.data[i];	
        		}
        	}    		
    	}
    	
//    	if(sendStatus_) {
    	
    	if (m_iKinema_result_inIn.isNew()) {
    		m_iKinema_result_inIn.read();
    		synchronized (kinematicsResult_) {
    			int i;
    			for (i=0; i<m_iKinema_result_in.v.data.length; i++) {
    				kinematicsResult_[i] = m_iKinema_result_in.v.data[i];
    			}
    			if (i<kinematicsResult_.length) {
    				for (; i<kinematicsResult_.length; i++) {
    					kinematicsResult_[i] = 0.0;
    				}
    			}
    							
    		}
    		sendStatus_ = sendSegmentData(kinematicsResult_);    		
    	}
    	
//    	}
//    	else
//    	{
//       		sendStatus_ = sendSegmentData(kinematicsResult_);    		
//    	}
    	    	
//    	if(sendStatus_){
    	imInterpolator.doSegment();
//    	}
 
    	
        return super.onExecute(ec_id);
    }

    /***
     *
     * The aborting action when main logic error occurred.
     * former rtc_aborting_entry()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//  @Override
//  public ReturnCode_t onAborting(int ec_id) {
//      return super.onAborting(ec_id);
//  }

    /***
     *
     * The error action in ERROR state
     * former rtc_error_do()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    public ReturnCode_t onError(int ec_id) {
//        return super.onError(ec_id);
//    }

    /***
     *
     * The reset action that is invoked resetting
     * This is same but different the former rtc_init_entry()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onReset(int ec_id) {
//        return super.onReset(ec_id);
//    }

    /***
     *
     * The state update action that is invoked after onExecute() action
     * no corresponding operation exists in OpenRTm-aist-0.2.0
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onStateUpdate(int ec_id) {
//        return super.onStateUpdate(ec_id);
//    }

    /***
     *
     * The action that is invoked when execution context's rate is changed
     * no corresponding operation exists in OpenRTm-aist-0.2.0
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onRateChanged(int ec_id) {
//        return super.onRateChanged(ec_id);
//    }
//
	// Configuration variable declaration
	// <rtc-template block="config_declare">
    /*!
     * 
     * - Name:  configFile
     * - DefaultValue: default.conf
     */
    protected StringHolder m_configFile = new StringHolder();
	// </rtc-template>

    // DataInPort declaration
    // <rtc-template block="inport_declare">
    protected TimedDoubleSeq m_arm_cartesian_fb_in_val;
    protected DataRef<TimedDoubleSeq> m_arm_cartesian_fb_in;
    /*!
     * control point current position (right arm, left arm)
     */
    protected InPort<TimedDoubleSeq> m_arm_cartesian_fb_inIn;

    protected TimedDoubleSeq m_iKinema_result_in_val;
    protected DataRef<TimedDoubleSeq> m_iKinema_result_in;
    /*!
     * inverse kinematics result.
     * (vehicle, lumbar, right arm, left arm position)
     */
    protected InPort<TimedDoubleSeq> m_iKinema_result_inIn;

    protected TimedDoubleSeq m_redundant_fb_in_val;
    protected DataRef<TimedDoubleSeq> m_redundant_fb_in;
    /*!
     * redundant axes(elbow) current postion (theta)
     */
    protected InPort<TimedDoubleSeq> m_redundant_fb_inIn;

    protected TimedDoubleSeq m_joint_fb_in_val;
    protected DataRef<TimedDoubleSeq> m_joint_fb_in;
    /*!
     * axes current position (theta)
     */
    protected InPort<TimedDoubleSeq> m_joint_fb_inIn;

    protected TimedDoubleSeq m_status_in_val;
    protected DataRef<TimedDoubleSeq> m_status_in;
    /*!
     * redundancy module status
     */
    protected InPort<TimedDoubleSeq> m_status_inIn;

    
    // </rtc-template>

    // DataOutPort declaration
    // <rtc-template block="outport_declare">
    protected TimedJointPos m_armR_joint_cmd_out_val;
    protected DataRef<TimedJointPos> m_armR_joint_cmd_out;
    /*!
     * right arm command position
     */
    protected OutPort<TimedJointPos> m_armR_joint_cmd_outOut;

    protected TimedJointPos m_armL_joint_cmd_out_val;
    protected DataRef<TimedJointPos> m_armL_joint_cmd_out;
    /*!
     * left arm command position
     */
    protected OutPort<TimedJointPos> m_armL_joint_cmd_outOut;

    protected TimedJointPos m_lumbar_cmd_out_val;
    protected DataRef<TimedJointPos> m_lumbar_cmd_out;
    /*!
     * lumbar command position
     */
    protected OutPort<TimedJointPos> m_lumbar_cmd_outOut;

    protected TimedDoubleSeq m_vehicle_cmd_out_val;
    protected DataRef<TimedDoubleSeq> m_vehicle_cmd_out;
    /*!
     * vehicle command position
     */
    protected OutPort<TimedDoubleSeq> m_vehicle_cmd_outOut;

    protected TimedDoubleSeq m_arm_cartesian_cmd_out_val;
    protected DataRef<TimedDoubleSeq> m_arm_cartesian_cmd_out;
    /*!
     */
    protected OutPort<TimedDoubleSeq> m_arm_cartesian_cmd_outOut;

    protected TimedDoubleSeq m_redundant_cmd_out_val;
    protected DataRef<TimedDoubleSeq> m_redundant_cmd_out;
    /*!
     * redundant axes command position.
     * (vehicle, lumbar, right elbow, left elbow)
     */
    protected OutPort<TimedDoubleSeq> m_redundant_cmd_outOut;

    
    // </rtc-template>

    // CORBA Port declaration
    // <rtc-template block="corbaport_declare">
    /*!
     */
    protected CorbaPort m_controllerPort;
    /*!
     */
    protected CorbaPort m_toRedundancyPort;
    /*!
     */
    protected CorbaPort m_armL_servicePort;
    /*!
     */
    protected CorbaPort m_armR_servicePort;
    
    /*!
     */
    protected CorbaPort m_lumbar_servicePort;

    /*!
     */
    protected CorbaPort m_vehicle_servicePort;

    // </rtc-template>
    protected IntegratedMotionInterpolator imInterpolator = new IntegratedMotionInterpolator(this);

    // Service declaration
    // <rtc-template block="service_declare">
    /*!
     */
    protected IntegratedMotionInterfaceSVC_impl m_toClient = new IntegratedMotionInterfaceSVC_impl(imInterpolator, this);
    
    // </rtc-template>

    // Consumer declaration
    // <rtc-template block="consumer_declare">
    protected CorbaConsumer<RedundancyConfig> m_redundancyConfigBase = new CorbaConsumer<RedundancyConfig>(RedundancyConfig.class);
    /*!
     */
    protected RedundancyConfig m_redundancyConfig;
    protected CorbaConsumer<ManipulatorCommonInterface_Common> m_armLBase = new CorbaConsumer<ManipulatorCommonInterface_Common>(ManipulatorCommonInterface_Common.class);
    /*!
     */
    protected ManipulatorCommonInterface_Common m_armL;
    protected CorbaConsumer<ManipulatorCommonInterface_Common> m_armRBase = new CorbaConsumer<ManipulatorCommonInterface_Common>(ManipulatorCommonInterface_Common.class);
    /*!
     */
    protected ManipulatorCommonInterface_Common m_armR;

    /*!
     */
    protected CorbaConsumer<LumbarUnit> m_lumbarBase = new CorbaConsumer<LumbarUnit>(LumbarUnit.class);
    protected LumbarUnit m_lumbar;
    
    /*!
     */
    protected CorbaConsumer<VehicleService> m_vehicleBase = new CorbaConsumer<VehicleService>(VehicleService.class);
    protected VehicleService m_vehicle;
    
    
    public ReturnID abort() {
		System.out.println("abort()");
		ReturnID ret = new ReturnID();

		boolean result = false;
		result = imInterpolator.abort();

		if(result)
			ret.id = 0;
		else
			ret.id = -1;
			
		return ret;
		
    }

	public ReturnID clearAlarms() {
		System.out.println("clearAlarms()");
		ReturnID ret = new ReturnID();

		imInterpolator.clearAlarm();
		
		RETURN_ID rtn = new RETURN_ID();
		rtn = m_armR.clearAlarms();
		
		if( m_lumbar != null) {
			m_lumbar.clearAlarms();
		}
		if( m_vehicle != null) {
			m_vehicle.clearAlarm();
		}

		ret.id = rtn.id;
		ret.comment = rtn.comment;
		
		return ret;
	}

	public ReturnID getActiveAlarm(AlarmSeqHolder alarms) {
		// TODO Auto-generated method stub
		return null;
	}

	public ReturnID getFeedbackPosCartesian(RightLeft type,
			FrameType frameType, CarPosWithElbowHolder pos) {
		
		ReturnID ret = new ReturnID(0,"");

		int index = 0;		
		synchronized (cartesianFeedback_) {
			if (type == RightLeft.LEFT) {
				index = 12;
			}			
			pos.value.carPos[0][3] = cartesianFeedback_[index];   //!< x
			pos.value.carPos[1][3] = cartesianFeedback_[++index]; //!< y
			pos.value.carPos[2][3] = cartesianFeedback_[++index]; //!< z
			
			for (int i=0; i<3; i++) {
				for (int j=0; j<3; j++) {
					index++;
					pos.value.carPos[i][j] = cartesianFeedback_[index]; //!< HgMat[i][j]					
				}
			}
			
		}

		synchronized (redundantFeedback_) {
			if (type == RightLeft.LEFT) {
				pos.value.elbow = redundantFeedback_[7];
			} else {
				pos.value.elbow = redundantFeedback_[6];
			}
					
		}
				
		return ret;
		
	}

	public ReturnID getFeedbackPosJoint(UnitType type, double[] pos) {

		ReturnID ret = new ReturnID(0,"");

		if (type == UnitType.RIGHT_ARM) {			
			for(int i=0; i<7; i++){
				pos[i] = armR_jointFeedback_[i];
			}
		}
		else if (type == UnitType.LEFT_ARM) {
			for(int i=0; i<7; i++){
				pos[i] = armL_jointFeedback_[i];
			}
		}
		else if (type == UnitType.LUMBER) {
			for(int i=0; i<3; i++){
				pos[i] = lumbar_jointFeedback_[i];
			}
		}
		else if (type == UnitType.VEHICLE) {
			for(int i=0; i<3; i++){
				pos[i] = vehicle_Feedback_[i];
			}			
		}
		else {
			ret.id = -1;
			ret.comment = "unit type error";			
		}
		
		return ret;
		}

	public ReturnID getRedundantPos(double[] redPos) {
		synchronized (redundantFeedback_) {			
			for(int i=0; i<redundantFeedback_.length; i++) {
				redPos[i] = redundantFeedback_[i]; 				
			}
		}
		
		ReturnID ret = new ReturnID();
		ret.id = 0;
		ret.comment = "";
		
		return ret;
		
	}

	/**
	 * 逆運動学演算結果を取得する
	 */
	public ReturnID getIKinematicsResult(double[] ikinema) {
		synchronized (kinematicsResult_) {			
			for(int i=0; i<kinematicsResult_.length; i++) {
				ikinema[i] = kinematicsResult_[i]; 				
			}
		}
		
		ReturnID ret = new ReturnID();
		ret.id = 0;
		ret.comment = "";
		
		return ret;
		
	}
	
	
	public ReturnID getState(IntHolder statusId,
			org.omg.CORBA.StringHolder message) {
		// TODO Auto-generated method stub
		return null;
	}

	public ReturnID getVersion(org.omg.CORBA.StringHolder version) {
		System.out.println("getVersion()");
		version.value = "1.0.0";

		ReturnID ret = new ReturnID();
		ret.id = 0;
		ret.comment = "";
		
		return ret;
	}

	public boolean isMoving() {
		// TODO Auto-generated method stub
		return false;
	}

	public boolean isServoOn() {
		return isServoOn_;
	}

	public ReturnID pause() {
		System.out.println("pause()");
		ReturnID ret = new ReturnID();

		boolean result = false;
		result = imInterpolator.pause();

		if(result)
			ret.id = 0;
		else
			ret.id = -1;
			
		return ret;
	}

	public ReturnID resetOriginalFrame() {
		// TODO Auto-generated method stub
		return null;
	}

	public ReturnID resume() {
		System.out.println("resume()");
		ReturnID ret = new ReturnID();

		boolean result = false;
		result = imInterpolator.resume();

		if(result)
			ret.id = 0;
		else
			ret.id = -1;
			
		return ret;
	}

	public ReturnID selectRedundantAxes(RedundantAxesMask redundancy) {
		
		System.out.println("selectRedundantAxes()");
		ReturnID ret = new ReturnID();
		
		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID rtn = 
			new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID();

		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundantAxesMask mask = 
			new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundantAxesMask();
		
		mask.rightElbow = redundancy.elbow;
		mask.leftElbow = redundancy.elbow;
		mask.lumbar1 = redundancy.lumbarRx;
		mask.lumbar2 = redundancy.lumbarRy;
		mask.lumbar3 = redundancy.lumbarRz;
		mask.vehicleX = redundancy.vehicleX;
		mask.vehicleY = redundancy.vehicleY;
		mask.vehicleTheta = redundancy.vehicleTheta;

		if(m_redundancyConfig == null) {
			System.out.println("service port is unconnected.");
			ret.id = -1;
			ret.comment = rtn.comment;
			return ret;			
		}
		
		rtn = m_redundancyConfig.selectRedundancy(mask);
		
		if(rtn.id != 0) {
			ret.id = rtn.id;
			ret.comment = rtn.comment;
			return ret;
		}
		
		ret.id = rtn.id;
		ret.comment = rtn.comment;
        
		return ret;
	}

	public ReturnID selectRedundantLimit(RedundantLimit limit) {
		
		System.out.println("selectRedundantLimit()");
		ReturnID ret = new ReturnID();
		
		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID rtn = 
			new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID();

		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundantLimit lim =
			new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundantLimit();
		
		lim.rightElbow = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.leftElbow = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.lumbar1 = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.lumbar2 = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.lumbar3 = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.vehicleX = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.vehicleY = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.vehicleTheta = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
        lim.rightElbow.upper = limit.rightElbow.upper;
        lim.rightElbow.lower = limit.rightElbow.lower;
        lim.leftElbow.upper = limit.leftElbow.upper;
        lim.leftElbow.lower = limit.leftElbow.lower;
        lim.lumbar1.upper = limit.lumbarRx.upper;
        lim.lumbar1.lower = limit.lumbarRx.lower;
        lim.lumbar2.upper = limit.lumbarRy.upper;
        lim.lumbar2.lower = limit.lumbarRy.lower;
        lim.lumbar3.upper = limit.lumbarRz.upper;
        lim.lumbar3.lower = limit.lumbarRz.lower;
        lim.vehicleX.upper = limit.vehicleX.upper;
        lim.vehicleX.lower = limit.vehicleX.lower;
        lim.vehicleY.upper = limit.vehicleY.upper;
        lim.vehicleY.lower = limit.vehicleY.lower;
        lim.vehicleTheta.upper = limit.vehicleTheta.upper;
        lim.vehicleTheta.lower = limit.vehicleTheta.lower;

		if(m_redundancyConfig == null) {
			System.out.println("service port is unconnected.");
			ret.id = -1;
			ret.comment = rtn.comment;
			return ret;			
		}        
		rtn = m_redundancyConfig.setRedundantAxesLimit(lim);
		
		ret.id = rtn.id;
		ret.comment = rtn.comment;
        
		return ret;
	}
	
	public ReturnID servoOff() {
		System.out.println("servoOff()");
		ReturnID ret = new ReturnID();

		RETURN_ID rtn = new RETURN_ID(0,"");

		rtn.id = -1;
		if(m_armR != null) {
			rtn = m_armR.servoOFF();
		}		
		if(m_armL != null) {
			rtn = m_armL.servoOFF();
		}		

		if( m_lumbar != null) {
			m_lumbar.servoOff();
			if( m_vehicle != null) {
				m_vehicle.setServo(false);
			}
			m_lumbar.powerOff();
			if( m_vehicle != null) {
				m_vehicle.setPower(false);
			}
		}
		if(rtn.id == 0) {
			imInterpolator.setServo(false);
			imInterpolator.setAmp(false);			
			imInterpolator.getQ().clear();
			isServoOn_ = false;
		}		
		
		ret.id = rtn.id;
		ret.comment = rtn.comment;

		return ret;
	}

	public ReturnID servoOn() {
		System.out.println("servoOn()");
		ReturnID ret = new ReturnID();

		imInterpolator.getQ().clear();
		
		RETURN_ID rtn = new RETURN_ID(0,"");

		rtn.id = 0;
		//wata
		if(m_armR != null) {
			rtn = m_armR.servoON();
			
			ret.id = -1;
			ret.comment = "right arm servo on Failed";
			return ret;
		}		
		if(m_armL != null) {
			rtn = m_armL.servoON();
			
			ret.id = -1;
			ret.comment = "right arm servo on Failed";
			return ret;
			
		}		
		if( m_lumbar != null) {
			boolean res;
			
			m_lumbar.tuOn();
			if( m_vehicle != null) {
				m_vehicle.setPower(true);
			}
			res = m_lumbar.servoOn();
			if(!res)
			{
				ret.id = -1;
				ret.comment = "lumbar servo on Failed";
				return ret;				
			}
						
			if( m_vehicle != null) {
				res = m_vehicle.setServo(true);
				if(!res)
				{
					ret.id = -1;
					ret.comment = "vehicle servo on Failed";
					return ret;				
				}
				
			}
		}
		
		if(rtn.id == 0) {
			imInterpolator.setAmp(true);
			imInterpolator.setServo(true);
			imInterpolator.getQ().clear();
			isServoOn_ = true;
		}
		
		ret.id = rtn.id;
		ret.comment = rtn.comment;
				
		return ret;
	}

	/**
	 * アームの手先目標位置・姿勢（回転行列）を冗長性利用モジュールに出力する
	 * @param cartesianPos 左右アームの目標位置・姿勢
	 */	
	public ReturnID setCartesianCommand(double[] cartesianPos) {
		
		ReturnID ret = new ReturnID();
		ret.id = 0;
		ret.comment = "";
		
		m_arm_cartesian_cmd_out_val.data = Arrays.copyOfRange(cartesianPos, 0, 24);
		m_arm_cartesian_cmd_out_val.tm = new Time(0,0);
/*		
		if (false) {
			for (double dd: m_arm_cartesian_cmd_out_val.data) {
				System.out.print(" " + dd);

//				try {
//					outCarPos.write(String.valueOf(dd) + ",");
//				} catch (IOException e) {
//					e.printStackTrace();
//				}
			}
			System.out.println();
			
//			try {
//				outCarPos.newLine();
//			} catch (IOException e) {
				// TODO Auto-generated catch block
//				e.printStackTrace();
//			}
			
		}
*/		
		m_arm_cartesian_cmd_outOut.write();
				
		return ret;
		
	}

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

		ReturnID ret = new ReturnID();
		
		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft rl;
		
		if(type == RightLeft.RIGHT) {
			rl = jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft.RIGHT;
		} else {
			rl = jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft.LEFT;
		}
		
		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID rid = 
			new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID();

		if(m_redundancyConfig == null) {
			System.out.println("service port is unconnected.");
			ret.id = -1;
			ret.comment = rid.comment;
			return ret;			
		}        
		rid = m_redundancyConfig.setControlPointOffset(rl, offset);

		ret.id = rid.id;
		ret.comment = rid.comment;
		
		return ret;
		
	}

	public ReturnID setJogTimeout(int time) {
		// TODO Auto-generated method stub
		return null;
	}

	public ReturnID setJointCommand(UnitType type, double[] jointPos) {

		ReturnID ret = new ReturnID();
		ret.id = 0;
		ret.comment = "";

		double axesdata[] = new double[20];
		
		if (type == UnitType.RIGHT_ARM) {
			for(int i=0; i<armRJointNum_; i++){
				axesdata[i+6] = jointPos[i];
			}
			for(int i=0; i<armLJointNum_; i++){
				axesdata[i+13] = armL_jointFeedback_[i];
			}
			for(int i=0; i<lumbar_axes_; i++){
				axesdata[i+3] = lumbar_jointFeedback_[i];
			}
		}
		if (type == UnitType.LEFT_ARM) {
			for(int i=0; i<armRJointNum_; i++){
				axesdata[i+6] = armR_jointFeedback_[i];
			}
			for(int i=0; i<armLJointNum_; i++){
				axesdata[i+13] = jointPos[i];
			}
			for(int i=0; i<lumbar_axes_; i++){
				axesdata[i+3] = lumbar_jointFeedback_[i];
			}
		}
		if (type == UnitType.LUMBER) {
			for(int i=0; i<armRJointNum_; i++){
				axesdata[i+6] = armR_jointFeedback_[i];
			}
			for(int i=0; i<armLJointNum_; i++){
				axesdata[i+13] = armL_jointFeedback_[i];
			}
			for(int i=0; i<lumbar_axes_; i++){
				axesdata[i+3] = jointPos[i];
			}
		}
		
		sendSegmentData(axesdata);
/*		
		if (type == UnitType.RIGHT_ARM) {			
			for(int i=0; i<armRJointNum_; i++){
				m_armR_joint_cmd_out_val.pos[i] = jointPos[i];
			}
			m_armR_joint_cmd_out_val.tm = new Time(0,0);
			m_armR_joint_cmd_outOut.write();			
		}
		else if (type == UnitType.LEFT_ARM) {
			for(int i=0; i<armLJointNum_; i++){
				m_armL_joint_cmd_out_val.pos[i] = jointPos[i];
			}
			m_armL_joint_cmd_out_val.tm = new Time(0,0);
			m_armL_joint_cmd_outOut.write();			
		}
		else if (type == UnitType.LUMBER) {
			for(int i=0; i<lumbar_axes_; i++){
				m_lumbar_cmd_out_val.pos[i] = jointPos[i];
			}
			m_lumbar_cmd_out_val.tm = new Time(0,0);
			m_lumbar_cmd_outOut.write();
		}
		else if (type == UnitType.VEHICLE) {
			for(int i=0; i<vehicle_axes_; i++){
				m_vehicle_cmd_out_val.data[i] = jointPos[i];
			}
			m_vehicle_cmd_out_val.tm = new Time(0,0);								
		}
		else {
			ret.id = -1;
		}
*/		
/*
		if (false) {
			System.out.print("joint ref out");
			for (double dd: m_armR_joint_cmd_out_val.pos) {
				System.out.print(" " + dd);
			}
			System.out.println();
		}

		if (true) {
			for (double dd: m_armR_joint_cmd_out_val.pos) {
				try {
					outCarPos.write(String.valueOf(dd) + ",");
				} catch (IOException e) {
					e.printStackTrace();
				}
			}
			
			try {
				outCarPos.newLine();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			
		}		
*/		
		return ret;
				
/*		
		double [] pos = new double[8];

		for (int i=0; i<7; i++) {
			pos[i] = jointPos[i];
		}
		pos[7] = 0.0; 
		
		m_armR_joint_cmd_out_val.pos = pos;
		m_armR_joint_cmd_out_val.tm = new Time(0,0);		

		if (false) {
			System.out.print("joint ref out");
			for (double dd: m_armR_joint_cmd_out_val.pos) {
				System.out.print(" " + dd);
			}
			System.out.println();
		}
*/		
	
	}

	
	/**
	 * 冗長軸の目標位置を冗長性利用モジュールに出力する
	 * @param redundantPos 冗長軸の目標位置
	 */	
	public ReturnID setRedundantCommand(double[] redundantPos) {

		ReturnID ret = new ReturnID();
		ret.id = 0;
		ret.comment = "";

		//! vehicle x,y,theta,lumbar rx,ry,rz,right elbow,left elbow
		m_redundant_cmd_out_val.data = Arrays.copyOfRange(redundantPos, 0, 8);
/*		
		if (false) {
			System.out.print("redundant_cmd out");
			for (double dd: m_redundant_cmd_out_val.data) {
				System.out.print(" " + dd);
			}
			System.out.println();
		}
*/		
		m_redundant_cmd_outOut.write();
		
		return ret;
		
	}

	public ReturnID stop() {
		// TODO Auto-generated method stub
		return null;
	}
  
	public ReturnID selectMode(short mode, RightLeft type,
			boolean isHoldOtherArm) {
		
		System.out.println("selectMode()");
		ReturnID ret = new ReturnID();
		
		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID rtn;

		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft typearg;

		if(type == RightLeft.RIGHT){
			typearg = jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft.RIGHT;
		}
		else{
			typearg = jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft.LEFT;
		}

		if(m_redundancyConfig == null) {
			System.out.println("service port is unconnected.");
			ret.id = -1;
			return ret;			
		}        
		
		rtn = m_redundancyConfig.selectMode(mode, typearg, isHoldOtherArm);
				
		ret.id = rtn.id;
		ret.comment = rtn.comment;
		
		return ret;
		
	}
		
    // </rtc-template>
	boolean sendSegmentData(double[] data) {
		
		// データポートへ出力
		double [] armRjointPos = new double[8];
		double [] armLjointPos = new double[8];
		double [] lumbarjointPos = new double[3];
		
		for (int i=0; i<3; i++) {
			lumbarjointPos[i] = data[3+i];
		}

		for (int i=0; i<7; i++) {
			armRjointPos[i] = data[6+i];
		}

		for (int i=0; i<7; i++) {
			armLjointPos[i] = data[13+i];
		}
/*
		if (false) {
			for (double dd: m_armR_joint_cmd_out_val.pos) {
				try {
					outCarPos.write(String.valueOf(dd) + ",");
				} catch (IOException e) {
					e.printStackTrace();
				}
			}
			
			try {
				outCarPos.newLine();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			
		}
*/		
		boolean armOut = false;
		boolean lumOut = false;
				
		m_armR_joint_cmd_out_val.pos = armRjointPos;
		armOut = m_armR_joint_cmd_outOut.write();

		if(!armOut)
			return false;

		m_armL_joint_cmd_out_val.pos = armLjointPos;
		armOut = m_armL_joint_cmd_outOut.write();

		if(!armOut)
			return false;
		
		m_lumbar_cmd_out_val.pos = lumbarjointPos;
		lumOut = m_lumbar_cmd_outOut.write();

		if(!lumOut)
			return false;
		
//		System.out.println(System.currentTimeMillis());
		//System.out.println(System.nanoTime());
		return true;
	}

    class DataListener extends ConnectorDataListenerT<TimedDoubleSeq>{
        public DataListener(final String name){
            super(TimedDoubleSeq.class);
            m_name = name;
        }
		@Override
		public void operator(ConnectorInfo info, TimedDoubleSeq inData) {

			synchronized (kinematicsResult_) {
				int i;
				for (i=0; i<inData.data.length; i++) {
					kinematicsResult_[i] = inData.data[i];
				}
				if (i<kinematicsResult_.length) {
					for (; i<kinematicsResult_.length; i++) {
						kinematicsResult_[i] = 0.0;
					}
				}
								
			}
						
		}
        public String m_name;
		
    }

}
