note


naoの腕をJavaで動かすサンプル

Pythonで腕を動かすコードを参考に、Java版を作ってみました。
Naoの腕が動くコードです。




import java.util.ArrayList;

import com.aldebaran.proxy.*;

public class Test {
private static String NAOQI_IP = "127.0.0.1";
private static int NAOQI_PORT = 9559;

public static void main(String[] args){
Test
test = new Test();

}

public Test(){

doArmControl(
"LArm");

}

public void stiffnessOn(ALMotionProxy proxy){
Variant pNames =
new Variant(new String[]{"Body"});
Variant pStiffnessLists =
new Variant(new float[]{1.0f});
Variant pTimeLists =
new Variant(new float[]{1.0f});
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists);
}

public void doArmControl(String effectorName){
ALMotionProxy motionProxy =
new ALMotionProxy(NAOQI_IP,NAOQI_PORT);
ALRobotPostureProxy postureProxy =
new ALRobotPostureProxy(NAOQI_IP,NAOQI_PORT);

//Set NAO in stifness On
stiffnessOn(motionProxy);

//Send NAO to Pose Init
postureProxy.goToPosture(
"StandInit", 0.5f);
boolean isEnabled = false;
int space = 2; //motion.FRAME_ROBOT in python sdk is 2.Seesee pynaoqi-sdk/motion.py
float[] effectorInit = motionProxy.getPosition(effectorName,2,isEnabled);

//Active LArm Tracking
isEnabled =
true;
motionProxy.wbEnableEffectorControl(effectorName, isEnabled);

/**
#Example showing how to set position target for LArm
# The 3 coordinates are absolute LArm position in NAO_SPACE
# Position in meter in x, y and z axis.

# X Axis LArm Position feasible movement = [ +0.00, +0.12] meter
# Y Axis LArm Position feasible movement = [ -0.05, +0.10] meter
# Y Axis RArm Position feasible movement = [ -0.10, +0.05] meter
# Z Axis LArm Position feasible movement = [ -0.10, +0.10] meter
**/

float coef = 1.0f;
if(effectorName.equals("LArm")){
coef = 1.0f;
}
else if(effectorName.equals("RArm")){
coef = -1.0f;
}

float[][] targetCoordinateList = new float[][]{
{0.12f,0.00f*coef,0.00f},
{0.12f,0.00f*coef,-0.10f},
{0.12f,0.05f*coef,-0.10f},
{0.12f,0.05f*coef,0.10f},
{0.12f,-0.10f*coef,0.10f},
{0.12f,0.10f*coef,-0.10f},
{0.12f,0.00f*coef,-0.10f},
{0.12f,0.00f*coef,0.00f},
{0.00f,0.00f*coef,0.00f}
};

/**
# wbSetEffectorControl is a non blocking function
# time.sleep allow head go to his target
# The recommended minimum period between two successives set commands is
# 0.2 s.
**/
for(int i=0;ilength;i++){
Variant targetCoordinate =
new Variant(new float[]{targetCoordinateList[i][0]+effectorInit[0],
targetCoordinateList[i][1]+effectorInit[1],targetCoordinateList[i][2]+effectorInit[2]});
motionProxy.wbSetEffectorControl(effectorName, targetCoordinate);
System.
out.println("target"+i+": X:"+(targetCoordinateList[i][0]+effectorInit[0])+" Y:"
+(targetCoordinateList[i][1]+effectorInit[1]) +" Z:"+(targetCoordinateList[i][2]+effectorInit[2]));
try {
Thread.sleep(1000);
//original is 4 sec but it's too slow
}
catch (InterruptedException e) {
e.printStackTrace();
}
}

//Deactiate Head Tracking
isEnabled =
false;
motionProxy.wbEnableEffectorControl(effectorName,isEnabled);

}

}