From 5332aa9d8998da53d2a5aa75b84e9790fd30507b Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 23 Apr 2020 20:08:39 +0900 Subject: [PATCH] add HRP2JSK HRP2JSKNT HRP2JSKNTS --- .../config/HRP2JSKCustomizer.yaml | 11 + .../config/HRP2JSKNTCustomizer.yaml | 11 + .../config/HRP2JSKNTSCustomizer.yaml | 11 + hrpsys_choreonoid/src/JAXONCustomizer.cpp | 66 +-- hrpsys_choreonoid_tutorials/CMakeLists.txt | 29 ++ .../config/BodyRTC_HRP2JSK.RH.conf | 92 +++++ .../config/BodyRTC_HRP2JSKNT.RH.conf | 92 +++++ .../config/BodyRTC_HRP2JSKNTS.RH.conf | 92 +++++ .../config/HRP2JSKNTS_RH_FLAT.cnoid.in | 387 ++++++++++++++++++ .../config/HRP2JSKNT_RH_FLAT.cnoid.in | 387 ++++++++++++++++++ .../config/HRP2JSK_RH_FLAT.cnoid.in | 383 +++++++++++++++++ .../launch/hrp2jsk_choreonoid.launch | 110 +++++ .../launch/hrp2jsknt_choreonoid.launch | 110 +++++ .../launch/hrp2jsknts_choreonoid.launch | 110 +++++ .../scripts/hrp2_rh_setup.py | 94 +++++ 15 files changed, 1955 insertions(+), 30 deletions(-) create mode 100644 hrpsys_choreonoid/config/HRP2JSKCustomizer.yaml create mode 100644 hrpsys_choreonoid/config/HRP2JSKNTCustomizer.yaml create mode 100644 hrpsys_choreonoid/config/HRP2JSKNTSCustomizer.yaml create mode 100644 hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSK.RH.conf create mode 100644 hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf create mode 100644 hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_FLAT.cnoid.in create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_FLAT.cnoid.in create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_FLAT.cnoid.in create mode 100644 hrpsys_choreonoid_tutorials/launch/hrp2jsk_choreonoid.launch create mode 100644 hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch create mode 100644 hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch create mode 100755 hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py diff --git a/hrpsys_choreonoid/config/HRP2JSKCustomizer.yaml b/hrpsys_choreonoid/config/HRP2JSKCustomizer.yaml new file mode 100644 index 00000000..1194a156 --- /dev/null +++ b/hrpsys_choreonoid/config/HRP2JSKCustomizer.yaml @@ -0,0 +1,11 @@ +bush: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 2.5e3 + dampingR: 2.5 + +tilt_laser: + TILT_UPPER_BOUND: 1.35 + TILT_POSITIVE_SPEED: 1.0 + TILT_LOWER_BOUND: -0.7 + TILT_NEGATIVE_SPEED: -1.0 diff --git a/hrpsys_choreonoid/config/HRP2JSKNTCustomizer.yaml b/hrpsys_choreonoid/config/HRP2JSKNTCustomizer.yaml new file mode 100644 index 00000000..1194a156 --- /dev/null +++ b/hrpsys_choreonoid/config/HRP2JSKNTCustomizer.yaml @@ -0,0 +1,11 @@ +bush: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 2.5e3 + dampingR: 2.5 + +tilt_laser: + TILT_UPPER_BOUND: 1.35 + TILT_POSITIVE_SPEED: 1.0 + TILT_LOWER_BOUND: -0.7 + TILT_NEGATIVE_SPEED: -1.0 diff --git a/hrpsys_choreonoid/config/HRP2JSKNTSCustomizer.yaml b/hrpsys_choreonoid/config/HRP2JSKNTSCustomizer.yaml new file mode 100644 index 00000000..1194a156 --- /dev/null +++ b/hrpsys_choreonoid/config/HRP2JSKNTSCustomizer.yaml @@ -0,0 +1,11 @@ +bush: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 2.5e3 + dampingR: 2.5 + +tilt_laser: + TILT_UPPER_BOUND: 1.35 + TILT_POSITIVE_SPEED: 1.0 + TILT_LOWER_BOUND: -0.7 + TILT_NEGATIVE_SPEED: -1.0 diff --git a/hrpsys_choreonoid/src/JAXONCustomizer.cpp b/hrpsys_choreonoid/src/JAXONCustomizer.cpp index 5580a881..505abc63 100644 --- a/hrpsys_choreonoid/src/JAXONCustomizer.cpp +++ b/hrpsys_choreonoid/src/JAXONCustomizer.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -53,6 +54,7 @@ static BodyCustomizerInterface bodyCustomizerInterface; struct JointValSet { + int index; double* valuePtr; double* velocityPtr; double* torqueForcePtr; @@ -63,7 +65,7 @@ struct JAXONCustomizer BodyHandle bodyHandle; bool hasVirtualBushJoints; - JointValSet jointValSets[2][3]; + JointValSet jointValSets[4][6]; double springT; double dampingT; double springR; @@ -102,28 +104,26 @@ static const char** getTargetModelNames() static void getVirtualbushJoints(JAXONCustomizer* customizer, BodyHandle body) { - customizer->hasVirtualBushJoints = true; - - int bushIndices[2][3]; - - bushIndices[0][0] = bodyInterface->getLinkIndexFromName(body, "RLEG_BUSH_Z"); - bushIndices[0][1] = bodyInterface->getLinkIndexFromName(body, "RLEG_BUSH_ROLL"); - bushIndices[0][2] = bodyInterface->getLinkIndexFromName(body, "RLEG_BUSH_PITCH"); - bushIndices[1][0] = bodyInterface->getLinkIndexFromName(body, "LLEG_BUSH_Z"); - bushIndices[1][1] = bodyInterface->getLinkIndexFromName(body, "LLEG_BUSH_ROLL"); - bushIndices[1][2] = bodyInterface->getLinkIndexFromName(body, "LLEG_BUSH_PITCH"); - - for(int i=0; i < 2; ++i){ - for(int j=0; j < 3; ++j){ - int bushIndex = bushIndices[i][j]; - if(bushIndex < 0){ - std::cerr << "[Customizer] failed to find out : " << i << " " << j << std::endl; - customizer->hasVirtualBushJoints = false; - } else { - JointValSet& jointValSet = customizer->jointValSets[i][j]; - jointValSet.valuePtr = bodyInterface->getJointValuePtr(body, bushIndex); - jointValSet.velocityPtr = bodyInterface->getJointVelocityPtr(body, bushIndex); - jointValSet.torqueForcePtr = bodyInterface->getJointForcePtr(body, bushIndex); + customizer->hasVirtualBushJoints = false; + + static const char* limbs[] = {"RLEG", "LLEG", "RARM", "LARM"}; + static const char* types[] = {"X", "Y", "Z", "ROLL", "PITCH", "YAW"}; + char bush_name[30]; + + for(int i=0; i < 4; ++i){ + for(int j=0; j < 6; ++j){ + JointValSet& jointValSet = customizer->jointValSets[i][j]; + sprintf(bush_name, "%s_BUSH_%s", limbs[i], types[j]); + jointValSet.index = bodyInterface->getLinkIndexFromName(body, bush_name); + if(jointValSet.index < 0){ + std::cerr << "[Customizer] failed to find out : " << bush_name << std::endl; + }else{ + std::cerr << "[Customizer] find out : " << bush_name << std::endl; + customizer->hasVirtualBushJoints = true; + + jointValSet.valuePtr = bodyInterface->getJointValuePtr(body, jointValSet.index); + jointValSet.velocityPtr = bodyInterface->getJointVelocityPtr(body, jointValSet.index); + jointValSet.torqueForcePtr = bodyInterface->getJointForcePtr(body, jointValSet.index); } } } @@ -215,15 +215,21 @@ static void setVirtualJointForces(BodyCustomizerHandle customizerHandle) JAXONCustomizer* customizer = static_cast(customizerHandle); if(customizer->hasVirtualBushJoints){ - for(int i=0; i < 2; ++i){ - JointValSet& trans = customizer->jointValSets[i][0]; - *(trans.torqueForcePtr) = - customizer->springT * (*trans.valuePtr) - customizer->dampingT * (*trans.velocityPtr); - //std::cerr << i << " " << 0 << " " << *(trans.torqueForcePtr) << " = " << -customizer->springT << " x " << *trans.valuePtr << " + " << - customizer->dampingT << " x " << *trans.velocityPtr << std::endl; + for(int i=0; i < 4; ++i){ + for(int j=0; j < 3; ++j){ + JointValSet& trans = customizer->jointValSets[i][j]; + if(trans.index >= 0){ + *(trans.torqueForcePtr) = - customizer->springT * (*trans.valuePtr) - customizer->dampingT * (*trans.velocityPtr); + //std::cerr << i << " " << 0 << " " << *(trans.torqueForcePtr) << " = " << -customizer->springT << " x " << *trans.valuePtr << " + " << - customizer->dampingT << " x " << *trans.velocityPtr << std::endl; + } + } - for(int j=1; j < 3; ++j){ + for(int j=3; j < 6; ++j){ JointValSet& rot = customizer->jointValSets[i][j]; - *(rot.torqueForcePtr) = - customizer->springR * (*rot.valuePtr) - customizer->dampingR * (*rot.velocityPtr); - //std::cerr << i << " " << j << " " << *(rot.torqueForcePtr) << " = " << -customizer->springR << " x " << *rot.valuePtr << " + " << - customizer->dampingR << " x " << *rot.velocityPtr << std::endl; + if(rot.index >= 0){ + *(rot.torqueForcePtr) = - customizer->springR * (*rot.valuePtr) - customizer->dampingR * (*rot.velocityPtr); + //std::cerr << i << " " << j << " " << *(rot.torqueForcePtr) << " = " << -customizer->springR << " x " << *rot.valuePtr << " + " << - customizer->dampingR << " x " << *rot.velocityPtr << std::endl; + } } } } diff --git a/hrpsys_choreonoid_tutorials/CMakeLists.txt b/hrpsys_choreonoid_tutorials/CMakeLists.txt index 32f34626..9bdc1735 100644 --- a/hrpsys_choreonoid_tutorials/CMakeLists.txt +++ b/hrpsys_choreonoid_tutorials/CMakeLists.txt @@ -27,6 +27,14 @@ else() set (JSK_MODELS_DIR ${jsk_models_SOURCE_PREFIX}) endif() +find_package(hrp2_models QUIET) +if(NOT "${hrp2_models_FOUND}") + string(ASCII 27 Esc) + message(WARNING "${Esc}[1;33mPackage hrp2_models is not found, if you have right to access them please include source code in catkin workspace${Esc}[m") +else() + set (HRP2_MODELS_DIR ${hrp2_models_SOURCE_PREFIX}) +endif() + ################################ ## compile_openhrp_model ## Generate OpenHRP3 .xml and .conf file. @@ -189,6 +197,27 @@ if (${jsk_models_FOUND}) configure_file(${PROJECT_SOURCE_DIR}/config/CHIDORI_RH_FLAT.cnoid.in ${PROJECT_SOURCE_DIR}/config/CHIDORI_RH_FLAT.cnoid @ONLY) endif() +### +#HRP2JSK conid +### +if (${hrp2_models_FOUND}) +configure_file(${PROJECT_SOURCE_DIR}/config/HRP2JSK_RH_FLAT.cnoid.in ${PROJECT_SOURCE_DIR}/config/HRP2JSK_RH_FLAT.cnoid @ONLY) +endif() + +### +#HRP2JSKNT conid +### +if (${hrp2_models_FOUND}) +configure_file(${PROJECT_SOURCE_DIR}/config/HRP2JSKNT_RH_FLAT.cnoid.in ${PROJECT_SOURCE_DIR}/config/HRP2JSKNT_RH_FLAT.cnoid @ONLY) +endif() + +### +#HRP2JSKNTS conid +### +if (${hrp2_models_FOUND}) +configure_file(${PROJECT_SOURCE_DIR}/config/HRP2JSKNTS_RH_FLAT.cnoid.in ${PROJECT_SOURCE_DIR}/config/HRP2JSKNTS_RH_FLAT.cnoid @ONLY) +endif() + ### # scene.yaml ### diff --git a/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSK.RH.conf b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSK.RH.conf new file mode 100644 index 00000000..38c1677d --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSK.RH.conf @@ -0,0 +1,92 @@ +## +name-server = localhost:15005 +## +## PD Controller +## in: angleRef, angle +## out: torque +## +in-port = tauIn:JOINT_TORQUE +out-port = angleOut:JOINT_VALUE +out-port = qvel:JOINT_VELOCITY +out-port = torque:JOINT_TORQUE +# out-port = ddq:JOINT_ACCELERATION +connection = tauIn:RobotHardware_choreonoid0:torqueOut +connection = angleOut:RobotHardware_choreonoid0:angleIn +connection = qvel:RobotHardware_choreonoid0:qvel_sim +connection = torque:RobotHardware_choreonoid0:torque_sim +### +# debug ## ground truth robot potition +### +out-port = WAIST:WAIST:ABS_TRANSFORM +# out-port = headq:motor_joint:JOINT_VALUE +#### +# sensors +#### +out-port = rfsensor_sim:rfsensor:FORCE_SENSOR +out-port = lfsensor_sim:lfsensor:FORCE_SENSOR +out-port = rhsensor_sim:rhsensor:FORCE_SENSOR +out-port = lhsensor_sim:lhsensor:FORCE_SENSOR +out-port = gsensor_sim:gsensor:ACCELERATION_SENSOR2 +out-port = gyrometer_sim:gyrometer:RATE_GYRO_SENSOR2 +connection = rfsensor_sim:RobotHardware_choreonoid0:rfsensor_sim +connection = lfsensor_sim:RobotHardware_choreonoid0:lfsensor_sim +connection = rhsensor_sim:RobotHardware_choreonoid0:rhsensor_sim +connection = lhsensor_sim:RobotHardware_choreonoid0:lhsensor_sim +connection = gsensor_sim:RobotHardware_choreonoid0:gsensor_sim +connection = gyrometer_sim:RobotHardware_choreonoid0:gyrometer_sim +#### +# vision +#### +# out-port = HEAD_RANGE:HEAD_RANGE:RANGE_SENSOR +# out-port = HEAD_LEFT_DEPTH:HEAD_LEFT_CAMERA:CAMERA_RANGE +# out-port = HEAD_LEFT_CAMERA:HEAD_LEFT_CAMERA:CAMERA_IMAGE +# out-port = HEAD_RIGHT_CAMERA:HEAD_RIGHT_CAMERA:CAMERA_IMAGE +#out-port = CHEST_CAMERA:CHEST_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA:LARM_CAMERA:CAMERA_IMAGE +#out-port = RARM_CAMERA:RARM_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA_N:LARM_CAMERA_N:CAMERA_IMAGE +#out-port = RARM_CAMERA_N:RARM_CAMERA_N:CAMERA_IMAGE + +#### +# constraint +#### +# out-port = F_BODY:WAIST:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK0:CHEST_JOINT0:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK1:CHEST_JOINT1:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK2:CHEST_JOINT2:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK0:HEAD_JOINT0:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK1:HEAD_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK0:LARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_LINK1:LARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK2:LARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_LARM_LINK3:LARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_LARM_LINK4:LARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_LARM_LINK5:LARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_LARM_LINK6:LARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_LARM_LINK7:LARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER0:LARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER1:LARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK0:RARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_LINK1:RARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK2:RARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_RARM_LINK3:RARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_RARM_LINK4:RARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_RARM_LINK5:RARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_RARM_LINK6:RARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_RARM_LINK7:RARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER0:RARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER1:RARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK0:LLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK1:LLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK2:LLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK3:LLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK4:LLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_UPPER:LLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_LOWER:LLEG_BUSH_PITCH:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK0:RLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK1:RLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK2:RLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK3:RLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK4:RLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_UPPER:RLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_LOWER:RLEG_BUSH_PITCH:CONSTRAINT_FORCE diff --git a/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf new file mode 100644 index 00000000..9f5f0003 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf @@ -0,0 +1,92 @@ +## +name-server = localhost:15005 +## +## PD Controller +## in: angleRef, angle +## out: torque +## +in-port = tauIn:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_TORQUE +out-port = angleOut:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_VALUE +out-port = qvel:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_VELOCITY +out-port = torque:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_TORQUE +# out-port = ddq:JOINT_ACCELERATION +connection = tauIn:RobotHardware_choreonoid0:torqueOut +connection = angleOut:RobotHardware_choreonoid0:angleIn +connection = qvel:RobotHardware_choreonoid0:qvel_sim +connection = torque:RobotHardware_choreonoid0:torque_sim +### +# debug ## ground truth robot potition +### +out-port = WAIST:WAIST:ABS_TRANSFORM +# out-port = headq:motor_joint:JOINT_VALUE +#### +# sensors +#### +out-port = rfsensor_sim:rfsensor:FORCE_SENSOR +out-port = lfsensor_sim:lfsensor:FORCE_SENSOR +out-port = rhsensor_sim:rhsensor:FORCE_SENSOR +out-port = lhsensor_sim:lhsensor:FORCE_SENSOR +out-port = gsensor_sim:gsensor:ACCELERATION_SENSOR2 +out-port = gyrometer_sim:gyrometer:RATE_GYRO_SENSOR2 +connection = rfsensor_sim:RobotHardware_choreonoid0:rfsensor_sim +connection = lfsensor_sim:RobotHardware_choreonoid0:lfsensor_sim +connection = rhsensor_sim:RobotHardware_choreonoid0:rhsensor_sim +connection = lhsensor_sim:RobotHardware_choreonoid0:lhsensor_sim +connection = gsensor_sim:RobotHardware_choreonoid0:gsensor_sim +connection = gyrometer_sim:RobotHardware_choreonoid0:gyrometer_sim +#### +# vision +#### +# out-port = HEAD_RANGE:HEAD_RANGE:RANGE_SENSOR +# out-port = HEAD_LEFT_DEPTH:HEAD_LEFT_CAMERA:CAMERA_RANGE +# out-port = HEAD_LEFT_CAMERA:HEAD_LEFT_CAMERA:CAMERA_IMAGE +# out-port = HEAD_RIGHT_CAMERA:HEAD_RIGHT_CAMERA:CAMERA_IMAGE +#out-port = CHEST_CAMERA:CHEST_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA:LARM_CAMERA:CAMERA_IMAGE +#out-port = RARM_CAMERA:RARM_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA_N:LARM_CAMERA_N:CAMERA_IMAGE +#out-port = RARM_CAMERA_N:RARM_CAMERA_N:CAMERA_IMAGE + +#### +# constraint +#### +# out-port = F_BODY:WAIST:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK0:CHEST_JOINT0:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK1:CHEST_JOINT1:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK2:CHEST_JOINT2:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK0:HEAD_JOINT0:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK1:HEAD_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK0:LARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_LINK1:LARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK2:LARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_LARM_LINK3:LARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_LARM_LINK4:LARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_LARM_LINK5:LARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_LARM_LINK6:LARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_LARM_LINK7:LARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER0:LARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER1:LARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK0:RARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_LINK1:RARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK2:RARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_RARM_LINK3:RARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_RARM_LINK4:RARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_RARM_LINK5:RARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_RARM_LINK6:RARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_RARM_LINK7:RARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER0:RARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER1:RARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK0:LLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK1:LLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK2:LLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK3:LLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK4:LLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_UPPER:LLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_LOWER:LLEG_BUSH_PITCH:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK0:RLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK1:RLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK2:RLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK3:RLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK4:RLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_UPPER:RLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_LOWER:RLEG_BUSH_PITCH:CONSTRAINT_FORCE diff --git a/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf new file mode 100644 index 00000000..9f5f0003 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf @@ -0,0 +1,92 @@ +## +name-server = localhost:15005 +## +## PD Controller +## in: angleRef, angle +## out: torque +## +in-port = tauIn:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_TORQUE +out-port = angleOut:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_VALUE +out-port = qvel:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_VELOCITY +out-port = torque:RLEG_JOINT0,RLEG_JOINT1,RLEG_JOINT2,RLEG_JOINT3,RLEG_JOINT4,RLEG_JOINT5,RLEG_JOINT6,LLEG_JOINT0,LLEG_JOINT1,LLEG_JOINT2,LLEG_JOINT3,LLEG_JOINT4,LLEG_JOINT5,LLEG_JOINT6,CHEST_JOINT0,CHEST_JOINT1,HEAD_JOINT0,HEAD_JOINT1,RARM_JOINT0,RARM_JOINT1,RARM_JOINT2,RARM_JOINT3,RARM_JOINT4,RARM_JOINT5,RARM_JOINT6,RARM_JOINT7,LARM_JOINT0,LARM_JOINT1,LARM_JOINT2,LARM_JOINT3,LARM_JOINT4,LARM_JOINT5,LARM_JOINT6,LARM_JOINT7:JOINT_TORQUE +# out-port = ddq:JOINT_ACCELERATION +connection = tauIn:RobotHardware_choreonoid0:torqueOut +connection = angleOut:RobotHardware_choreonoid0:angleIn +connection = qvel:RobotHardware_choreonoid0:qvel_sim +connection = torque:RobotHardware_choreonoid0:torque_sim +### +# debug ## ground truth robot potition +### +out-port = WAIST:WAIST:ABS_TRANSFORM +# out-port = headq:motor_joint:JOINT_VALUE +#### +# sensors +#### +out-port = rfsensor_sim:rfsensor:FORCE_SENSOR +out-port = lfsensor_sim:lfsensor:FORCE_SENSOR +out-port = rhsensor_sim:rhsensor:FORCE_SENSOR +out-port = lhsensor_sim:lhsensor:FORCE_SENSOR +out-port = gsensor_sim:gsensor:ACCELERATION_SENSOR2 +out-port = gyrometer_sim:gyrometer:RATE_GYRO_SENSOR2 +connection = rfsensor_sim:RobotHardware_choreonoid0:rfsensor_sim +connection = lfsensor_sim:RobotHardware_choreonoid0:lfsensor_sim +connection = rhsensor_sim:RobotHardware_choreonoid0:rhsensor_sim +connection = lhsensor_sim:RobotHardware_choreonoid0:lhsensor_sim +connection = gsensor_sim:RobotHardware_choreonoid0:gsensor_sim +connection = gyrometer_sim:RobotHardware_choreonoid0:gyrometer_sim +#### +# vision +#### +# out-port = HEAD_RANGE:HEAD_RANGE:RANGE_SENSOR +# out-port = HEAD_LEFT_DEPTH:HEAD_LEFT_CAMERA:CAMERA_RANGE +# out-port = HEAD_LEFT_CAMERA:HEAD_LEFT_CAMERA:CAMERA_IMAGE +# out-port = HEAD_RIGHT_CAMERA:HEAD_RIGHT_CAMERA:CAMERA_IMAGE +#out-port = CHEST_CAMERA:CHEST_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA:LARM_CAMERA:CAMERA_IMAGE +#out-port = RARM_CAMERA:RARM_CAMERA:CAMERA_IMAGE +#out-port = LARM_CAMERA_N:LARM_CAMERA_N:CAMERA_IMAGE +#out-port = RARM_CAMERA_N:RARM_CAMERA_N:CAMERA_IMAGE + +#### +# constraint +#### +# out-port = F_BODY:WAIST:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK0:CHEST_JOINT0:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK1:CHEST_JOINT1:CONSTRAINT_FORCE +# out-port = F_CHEST_LINK2:CHEST_JOINT2:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK0:HEAD_JOINT0:CONSTRAINT_FORCE +# out-port = F_HEAD_LINK1:HEAD_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK0:LARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_LINK1:LARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_LARM_LINK2:LARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_LARM_LINK3:LARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_LARM_LINK4:LARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_LARM_LINK5:LARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_LARM_LINK6:LARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_LARM_LINK7:LARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER0:LARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_LARM_FINGER1:LARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK0:RARM_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_LINK1:RARM_JOINT1:CONSTRAINT_FORCE +# out-port = F_RARM_LINK2:RARM_JOINT2:CONSTRAINT_FORCE +# out-port = F_RARM_LINK3:RARM_JOINT3:CONSTRAINT_FORCE +# out-port = F_RARM_LINK4:RARM_JOINT4:CONSTRAINT_FORCE +# out-port = F_RARM_LINK5:RARM_JOINT5:CONSTRAINT_FORCE +# out-port = F_RARM_LINK6:RARM_JOINT6:CONSTRAINT_FORCE +# out-port = F_RARM_LINK7:RARM_JOINT7:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER0:RARM_F_JOINT0:CONSTRAINT_FORCE +# out-port = F_RARM_FINGER1:RARM_F_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK0:LLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK1:LLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK2:LLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK3:LLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK4:LLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_UPPER:LLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_LLEG_LINK5_LOWER:LLEG_BUSH_PITCH:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK0:RLEG_JOINT0:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK1:RLEG_JOINT1:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK2:RLEG_JOINT2:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK3:RLEG_JOINT3:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK4:RLEG_JOINT4:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_UPPER:RLEG_JOINT5:CONSTRAINT_FORCE +# out-port = F_RLEG_LINK5_LOWER:RLEG_BUSH_PITCH:CONSTRAINT_FORCE diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_FLAT.cnoid.in b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_FLAT.cnoid.in new file mode 100644 index 00000000..9c349862 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_FLAT.cnoid.in @@ -0,0 +1,387 @@ +items: + id: 0 + name: "Root" + plugin: Base + class: RootItem + children: + - + id: 1 + name: "World" + plugin: Body + class: WorldItem + data: + collisionDetection: false + collisionDetector: AISTCollisionDetector + children: + - + id: 2 + name: "HRP2JSK" + plugin: Body + class: BodyItem + data: + modelFile: "@HRP2_MODELS_DIR@/HRP2JSKNTS_WITH_3HAND_for_OpenHRP3/HRP2JSKNTSmain_bush.wrl" + currentBaseLink: "WAIST" + rootPosition: [ 0.0, 0.0, 0.650 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ + -0.000015, 0.003323, -0.468274, 0.872491, -0.404216, -0.003323, 0.000000, + -0.000015, 0.003323, -0.467353, 0.870656, -0.403303, -0.003323, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799 + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000] + initialRootPosition: [ 0.0, 0.0, 0.650 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ + -0.000015, 0.003323, -0.468274, 0.872491, -0.404216, -0.003323, 0.000000, + -0.000015, 0.003323, -0.467353, 0.870656, -0.403303, -0.003323, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799 + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + children: + - + id: 3 + name: "BodyRTC" + plugin: OpenRTM + class: BodyRTCItem + data: + isImmediateMode: true + moduleName: "@JVRC_RTC_DIRECTORY@/lib/RobotHardware_choreonoid" + confFileName: "@JVRC_CONF_DIRECTORY@/BodyRTC_HRP2JSKNTS.RH.conf" + configurationMode: Use Configuration File + AutoConnect: false + InstanceName: HRP2JSKNTS(Robot)0 + bodyPeriodicRate: 0.004 + - + id: 4 + name: "floor" + plugin: Body + class: BodyItem + data: + modelFile: "${SHARE}/model/misc/floor.body" + currentBaseLink: "" + rootPosition: [ 0, 0, -0.1 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ 0.000000 ] + initialRootPosition: [ 0, 0, -0.1 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ 0.000000 ] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + - + id: 5 + name: "AISTSimulator" + plugin: Body + class: AISTSimulatorItem + data: + realtimeSync: true + recording: full + timeRangeMode: TimeBar range + onlyActiveControlPeriod: true + timeLength: 12000 + allLinkPositionOutputMode: false + deviceStateOutput: true + controllerThreads: true + recordCollisionData: false + dynamicsMode: Forward dynamics + integrationMode: Runge Kutta + gravity: [ 0, 0, -9.80665 ] + staticFriction: 1 + slipFriction: 1 + cullingThresh: 0.005 + contactCullingDepth: 0.03 + errorCriterion: 0.001 + maxNumIterations: 1000 + contactCorrectionDepth: 0.0001 + contactCorrectionVelocityRatio: 1 + kinematicWalking: false + 2Dmode: false + - + id: 9 + name: "ros_service_server.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/scripts/ros_service_server.py" + executionOnLoading: true + backgroundExecution: false + +views: + - + id: 0 + name: "CameraImage" + plugin: Base + class: ImageView + mounted: true + - + id: 1 + plugin: Base + class: ItemPropertyView + mounted: true + - + id: 2 + plugin: Base + class: ItemTreeView + mounted: true + state: + selected: [ 9 ] + checked: [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, ] + expanded: [ 1 ] + - + id: 3 + plugin: Base + class: MessageView + mounted: true + - + id: 4 + plugin: Base + class: SceneView + mounted: true + state: + editMode: true + viewpointControlMode: thirdPerson + collisionLines: false + polygonMode: fill + defaultHeadLight: true + defaultHeadLightIntensity: 0.75 + headLightLightingFromBack: false + worldLight: true + worldLightIntensity: 0.5 + worldLightAmbient: 0.3 + additionalLights: true + floorGrid: true + floorGridSpan: 10 + floorGridInterval: 0.5 + xzGridSpan: 10 + xzGridInterval: 0.5 + xzGrid: false + yzGridSpan: 10 + yzGridInterval: 0.5 + texture: true + lineWidth: 1 + pointSize: 1 + normalVisualization: false + normalLength: 0.01 + coordinateAxes: true + showFPS: false + enableNewDisplayListDoubleRendering: false + useBufferForPicking: true + cameras: + - + camera: [ System, Perspective ] + isCurrent: true + fieldOfView: 0.6978 + near: 0.01 + far: 100 + eye: [ 2.7, -2.7, 2 ] + direction: [ -0.7, 0.7, -0.3 ] + up: [ -0.25, 0.25, 1] + - + camera: [ System, Orthographic ] + orthoHeight: 20 + near: 0.01 + far: 100 + backgroundColor: [ 0.100000001, 0.100000001, 0.300000012 ] + gridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + xzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + yzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + dedicatedItemTreeViewChecks: false + - + id: 5 + name: "Task" + plugin: Base + class: TaskView + state: + layoutMode: horizontal + isAutoMode: false + - + id: 6 + plugin: Body + class: BodyLinkView + mounted: true + state: + showRotationMatrix: false + - + id: 7 + plugin: Body + class: JointSliderView + mounted: true + state: + showAllJoints: true + jointId: false + name: true + numColumns: 1 + spinBox: true + slider: true + labelOnLeft: true + - + id: 8 + plugin: Body + class: LinkSelectionView + mounted: true + state: + listingMode: "Link List" + - + id: 10 + plugin: Python + class: PythonConsoleView + mounted: true +toolbars: + "TimeBar": + minTime: 0 + maxTime: 12000 + frameRate: 1000 + playbackFrameRate: 50 + idleLoopDrivenMode: false + currentTime: 0 + speedScale: 1 + syncToOngoingUpdates: true + autoExpansion: true + "KinematicsBar": + mode: AUTO + enablePositionDragger: true + penetrationBlock: false + collisionLinkHighlight: false + snapDistance: 0.025 + penetrationBlockDepth: 0.0005 + lazyCollisionDetectionMode: true + "LeggedBodyBar": + stanceWidth: 0.15 + "BodyMotionGenerationBar": + autoGenerationForNewBody: true + balancer: false + autoGeneration: false + timeScaleRatio: 1 + preInitialDuration: 1 + postFinalDuration: 1 + onlyTimeBarRange: false + makeNewBodyItem: true + stealthyStepMode: true + stealthyHeightRatioThresh: 2 + flatLiftingHeight: 0.005 + flatLandingHeight: 0.005 + impactReductionHeight: 0.005 + impactReductionTime: 0.04 + autoZmp: true + minZmpTransitionTime: 0.1 + zmpCenteringTimeThresh: 0.03 + zmpTimeMarginBeforeLiftingSpin: 0 + zmpMaxDistanceFromCenter: 0.02 + allLinkPositions: false + lipSyncMix: false + timeToStartBalancer: 0 + balancerIterations: 2 + plainBalancerMode: false + boundaryConditionType: position + boundarySmootherType: quintic + boundarySmootherTime: 0.5 + boundaryCmAdjustment: false + boundaryCmAdjustmentTime: 1 + waistHeightRelaxation: false + gravity: 9.8 + dynamicsTimeRatio: 1 +Body: + "BodyMotionEngine": + updateJointVelocities: false + "EditableSceneBody": + editableSceneBodies: + - + bodyItem: 2 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 3 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 4 + showCenterOfMass: false + showPpcom: false + showZmp: false + staticModelEditing: false + "KinematicFaultChecker": + checkJointPositions: true + angleMargin: 0 + translationMargin: 0 + checkJointVelocities: true + velocityLimitRatio: 100 + targetJoints: all + checkSelfCollisions: true + onlyTimeBarRange: false +OpenRTM: + "deleteUnmanagedRTCsOnStartingSimulation": false +viewAreas: + - + type: embedded + tabs: true + contents: + type: splitter + orientation: horizontal + sizes: [ 310, 2195 ] + children: + - + type: splitter + orientation: vertical + sizes: [ 768, 767 ] + children: + - + type: pane + views: [ 2 ] + current: 2 + - + type: pane + views: [ 1, 8 ] + current: 1 + - + type: splitter + orientation: vertical + sizes: [ 1205, 330 ] + children: + - + type: splitter + orientation: horizontal + sizes: [ 449, 1740 ] + children: + - + type: pane + views: [ 6, 7, 0 ] + current: 6 + - + type: pane + views: [ 4 ] + current: 4 + - + type: pane + views: [ 3, 10 ] + current: 10 +layoutOfToolBars: + rows: + - + - { name: "FileBar", x: 0, priority: 0 } + - { name: "ScriptBar", x: 47, priority: 3 } + - { name: "TimeBar", x: 47, priority: 1 } + - { name: "SceneBar", x: 1455, priority: 2 } + - { name: "SimulationBar", x: 1464, priority: 0 } diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_FLAT.cnoid.in b/hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_FLAT.cnoid.in new file mode 100644 index 00000000..cc9a3f41 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_FLAT.cnoid.in @@ -0,0 +1,387 @@ +items: + id: 0 + name: "Root" + plugin: Base + class: RootItem + children: + - + id: 1 + name: "World" + plugin: Body + class: WorldItem + data: + collisionDetection: false + collisionDetector: AISTCollisionDetector + children: + - + id: 2 + name: "HRP2JSK" + plugin: Body + class: BodyItem + data: + modelFile: "@HRP2_MODELS_DIR@/HRP2JSKNT_WITH_3HAND_for_OpenHRP3/HRP2JSKNTmain_bush.wrl" + currentBaseLink: "WAIST" + rootPosition: [ 0.0, 0.0, 0.650 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ + -0.000015, 0.003323, -0.468274, 0.872491, -0.404216, -0.003323, 0.000000, + -0.000015, 0.003323, -0.467353, 0.870656, -0.403303, -0.003323, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799 + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000] + initialRootPosition: [ 0.0, 0.0, 0.650 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ + -0.000015, 0.003323, -0.468274, 0.872491, -0.404216, -0.003323, 0.000000, + -0.000015, 0.003323, -0.467353, 0.870656, -0.403303, -0.003323, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799 + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + children: + - + id: 3 + name: "BodyRTC" + plugin: OpenRTM + class: BodyRTCItem + data: + isImmediateMode: true + moduleName: "@JVRC_RTC_DIRECTORY@/lib/RobotHardware_choreonoid" + confFileName: "@JVRC_CONF_DIRECTORY@/BodyRTC_HRP2JSKNT.RH.conf" + configurationMode: Use Configuration File + AutoConnect: false + InstanceName: HRP2JSKNT(Robot)0 + bodyPeriodicRate: 0.004 + - + id: 4 + name: "floor" + plugin: Body + class: BodyItem + data: + modelFile: "${SHARE}/model/misc/floor.body" + currentBaseLink: "" + rootPosition: [ 0, 0, -0.1 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ 0.000000 ] + initialRootPosition: [ 0, 0, -0.1 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ 0.000000 ] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + - + id: 5 + name: "AISTSimulator" + plugin: Body + class: AISTSimulatorItem + data: + realtimeSync: true + recording: full + timeRangeMode: TimeBar range + onlyActiveControlPeriod: true + timeLength: 12000 + allLinkPositionOutputMode: false + deviceStateOutput: true + controllerThreads: true + recordCollisionData: false + dynamicsMode: Forward dynamics + integrationMode: Runge Kutta + gravity: [ 0, 0, -9.80665 ] + staticFriction: 1 + slipFriction: 1 + cullingThresh: 0.005 + contactCullingDepth: 0.03 + errorCriterion: 0.001 + maxNumIterations: 1000 + contactCorrectionDepth: 0.0001 + contactCorrectionVelocityRatio: 1 + kinematicWalking: false + 2Dmode: false + - + id: 9 + name: "ros_service_server.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/scripts/ros_service_server.py" + executionOnLoading: true + backgroundExecution: false + +views: + - + id: 0 + name: "CameraImage" + plugin: Base + class: ImageView + mounted: true + - + id: 1 + plugin: Base + class: ItemPropertyView + mounted: true + - + id: 2 + plugin: Base + class: ItemTreeView + mounted: true + state: + selected: [ 9 ] + checked: [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, ] + expanded: [ 1 ] + - + id: 3 + plugin: Base + class: MessageView + mounted: true + - + id: 4 + plugin: Base + class: SceneView + mounted: true + state: + editMode: true + viewpointControlMode: thirdPerson + collisionLines: false + polygonMode: fill + defaultHeadLight: true + defaultHeadLightIntensity: 0.75 + headLightLightingFromBack: false + worldLight: true + worldLightIntensity: 0.5 + worldLightAmbient: 0.3 + additionalLights: true + floorGrid: true + floorGridSpan: 10 + floorGridInterval: 0.5 + xzGridSpan: 10 + xzGridInterval: 0.5 + xzGrid: false + yzGridSpan: 10 + yzGridInterval: 0.5 + texture: true + lineWidth: 1 + pointSize: 1 + normalVisualization: false + normalLength: 0.01 + coordinateAxes: true + showFPS: false + enableNewDisplayListDoubleRendering: false + useBufferForPicking: true + cameras: + - + camera: [ System, Perspective ] + isCurrent: true + fieldOfView: 0.6978 + near: 0.01 + far: 100 + eye: [ 2.7, -2.7, 2 ] + direction: [ -0.7, 0.7, -0.3 ] + up: [ -0.25, 0.25, 1] + - + camera: [ System, Orthographic ] + orthoHeight: 20 + near: 0.01 + far: 100 + backgroundColor: [ 0.100000001, 0.100000001, 0.300000012 ] + gridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + xzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + yzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + dedicatedItemTreeViewChecks: false + - + id: 5 + name: "Task" + plugin: Base + class: TaskView + state: + layoutMode: horizontal + isAutoMode: false + - + id: 6 + plugin: Body + class: BodyLinkView + mounted: true + state: + showRotationMatrix: false + - + id: 7 + plugin: Body + class: JointSliderView + mounted: true + state: + showAllJoints: true + jointId: false + name: true + numColumns: 1 + spinBox: true + slider: true + labelOnLeft: true + - + id: 8 + plugin: Body + class: LinkSelectionView + mounted: true + state: + listingMode: "Link List" + - + id: 10 + plugin: Python + class: PythonConsoleView + mounted: true +toolbars: + "TimeBar": + minTime: 0 + maxTime: 12000 + frameRate: 1000 + playbackFrameRate: 50 + idleLoopDrivenMode: false + currentTime: 0 + speedScale: 1 + syncToOngoingUpdates: true + autoExpansion: true + "KinematicsBar": + mode: AUTO + enablePositionDragger: true + penetrationBlock: false + collisionLinkHighlight: false + snapDistance: 0.025 + penetrationBlockDepth: 0.0005 + lazyCollisionDetectionMode: true + "LeggedBodyBar": + stanceWidth: 0.15 + "BodyMotionGenerationBar": + autoGenerationForNewBody: true + balancer: false + autoGeneration: false + timeScaleRatio: 1 + preInitialDuration: 1 + postFinalDuration: 1 + onlyTimeBarRange: false + makeNewBodyItem: true + stealthyStepMode: true + stealthyHeightRatioThresh: 2 + flatLiftingHeight: 0.005 + flatLandingHeight: 0.005 + impactReductionHeight: 0.005 + impactReductionTime: 0.04 + autoZmp: true + minZmpTransitionTime: 0.1 + zmpCenteringTimeThresh: 0.03 + zmpTimeMarginBeforeLiftingSpin: 0 + zmpMaxDistanceFromCenter: 0.02 + allLinkPositions: false + lipSyncMix: false + timeToStartBalancer: 0 + balancerIterations: 2 + plainBalancerMode: false + boundaryConditionType: position + boundarySmootherType: quintic + boundarySmootherTime: 0.5 + boundaryCmAdjustment: false + boundaryCmAdjustmentTime: 1 + waistHeightRelaxation: false + gravity: 9.8 + dynamicsTimeRatio: 1 +Body: + "BodyMotionEngine": + updateJointVelocities: false + "EditableSceneBody": + editableSceneBodies: + - + bodyItem: 2 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 3 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 4 + showCenterOfMass: false + showPpcom: false + showZmp: false + staticModelEditing: false + "KinematicFaultChecker": + checkJointPositions: true + angleMargin: 0 + translationMargin: 0 + checkJointVelocities: true + velocityLimitRatio: 100 + targetJoints: all + checkSelfCollisions: true + onlyTimeBarRange: false +OpenRTM: + "deleteUnmanagedRTCsOnStartingSimulation": false +viewAreas: + - + type: embedded + tabs: true + contents: + type: splitter + orientation: horizontal + sizes: [ 310, 2195 ] + children: + - + type: splitter + orientation: vertical + sizes: [ 768, 767 ] + children: + - + type: pane + views: [ 2 ] + current: 2 + - + type: pane + views: [ 1, 8 ] + current: 1 + - + type: splitter + orientation: vertical + sizes: [ 1205, 330 ] + children: + - + type: splitter + orientation: horizontal + sizes: [ 449, 1740 ] + children: + - + type: pane + views: [ 6, 7, 0 ] + current: 6 + - + type: pane + views: [ 4 ] + current: 4 + - + type: pane + views: [ 3, 10 ] + current: 10 +layoutOfToolBars: + rows: + - + - { name: "FileBar", x: 0, priority: 0 } + - { name: "ScriptBar", x: 47, priority: 3 } + - { name: "TimeBar", x: 47, priority: 1 } + - { name: "SceneBar", x: 1455, priority: 2 } + - { name: "SimulationBar", x: 1464, priority: 0 } diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_FLAT.cnoid.in b/hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_FLAT.cnoid.in new file mode 100644 index 00000000..a5c647c8 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_FLAT.cnoid.in @@ -0,0 +1,383 @@ +items: + id: 0 + name: "Root" + plugin: Base + class: RootItem + children: + - + id: 1 + name: "World" + plugin: Body + class: WorldItem + data: + collisionDetection: false + collisionDetector: AISTCollisionDetector + children: + - + id: 2 + name: "HRP2JSK" + plugin: Body + class: BodyItem + data: + modelFile: "@HRP2_MODELS_DIR@/HRP2JSK_for_OpenHRP3/HRP2JSKmain_bush.wrl" + currentBaseLink: "WAIST" + rootPosition: [ 0.0, 0.0, 0.650 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ + -0.000015, 0.003323, -0.468274, 0.872491, -0.404216, -0.003323, + -0.000015, 0.003323, -0.467353, 0.870656, -0.403303, -0.003323, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799] + initialRootPosition: [ 0.0, 0.0, 0.650 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ + -0.000015, 0.003323, -0.468274, 0.872491, -0.404216, -0.003323, + -0.000015, 0.003323, -0.467353, 0.870656, -0.403303, -0.003323, + 0.000000, 0.000000, 0.000000, 0.000000, + 0.174533, -0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, 0.261799, + 0.174533, 0.174533, 0.000000, -0.436332, 0.000000, 0.000000, -0.174533, -0.261799] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + children: + - + id: 3 + name: "BodyRTC" + plugin: OpenRTM + class: BodyRTCItem + data: + isImmediateMode: true + moduleName: "@JVRC_RTC_DIRECTORY@/lib/RobotHardware_choreonoid" + confFileName: "@JVRC_CONF_DIRECTORY@/BodyRTC_HRP2JSK.RH.conf" + configurationMode: Use Configuration File + AutoConnect: false + InstanceName: HRP2JSK(Robot)0 + bodyPeriodicRate: 0.004 + - + id: 4 + name: "floor" + plugin: Body + class: BodyItem + data: + modelFile: "${SHARE}/model/misc/floor.body" + currentBaseLink: "" + rootPosition: [ 0, 0, -0.1 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ 0.000000 ] + initialRootPosition: [ 0, 0, -0.1 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ 0.000000 ] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + - + id: 5 + name: "AISTSimulator" + plugin: Body + class: AISTSimulatorItem + data: + realtimeSync: true + recording: full + timeRangeMode: TimeBar range + onlyActiveControlPeriod: true + timeLength: 12000 + allLinkPositionOutputMode: false + deviceStateOutput: true + controllerThreads: true + recordCollisionData: false + dynamicsMode: Forward dynamics + integrationMode: Runge Kutta + gravity: [ 0, 0, -9.80665 ] + staticFriction: 1 + slipFriction: 1 + cullingThresh: 0.005 + contactCullingDepth: 0.03 + errorCriterion: 0.001 + maxNumIterations: 1000 + contactCorrectionDepth: 0.0001 + contactCorrectionVelocityRatio: 1 + kinematicWalking: false + 2Dmode: false + - + id: 9 + name: "ros_service_server.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/scripts/ros_service_server.py" + executionOnLoading: true + backgroundExecution: false + +views: + - + id: 0 + name: "CameraImage" + plugin: Base + class: ImageView + mounted: true + - + id: 1 + plugin: Base + class: ItemPropertyView + mounted: true + - + id: 2 + plugin: Base + class: ItemTreeView + mounted: true + state: + selected: [ 9 ] + checked: [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, ] + expanded: [ 1 ] + - + id: 3 + plugin: Base + class: MessageView + mounted: true + - + id: 4 + plugin: Base + class: SceneView + mounted: true + state: + editMode: true + viewpointControlMode: thirdPerson + collisionLines: false + polygonMode: fill + defaultHeadLight: true + defaultHeadLightIntensity: 0.75 + headLightLightingFromBack: false + worldLight: true + worldLightIntensity: 0.5 + worldLightAmbient: 0.3 + additionalLights: true + floorGrid: true + floorGridSpan: 10 + floorGridInterval: 0.5 + xzGridSpan: 10 + xzGridInterval: 0.5 + xzGrid: false + yzGridSpan: 10 + yzGridInterval: 0.5 + texture: true + lineWidth: 1 + pointSize: 1 + normalVisualization: false + normalLength: 0.01 + coordinateAxes: true + showFPS: false + enableNewDisplayListDoubleRendering: false + useBufferForPicking: true + cameras: + - + camera: [ System, Perspective ] + isCurrent: true + fieldOfView: 0.6978 + near: 0.01 + far: 100 + eye: [ 2.7, -2.7, 2 ] + direction: [ -0.7, 0.7, -0.3 ] + up: [ -0.25, 0.25, 1] + - + camera: [ System, Orthographic ] + orthoHeight: 20 + near: 0.01 + far: 100 + backgroundColor: [ 0.100000001, 0.100000001, 0.300000012 ] + gridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + xzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + yzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ] + dedicatedItemTreeViewChecks: false + - + id: 5 + name: "Task" + plugin: Base + class: TaskView + state: + layoutMode: horizontal + isAutoMode: false + - + id: 6 + plugin: Body + class: BodyLinkView + mounted: true + state: + showRotationMatrix: false + - + id: 7 + plugin: Body + class: JointSliderView + mounted: true + state: + showAllJoints: true + jointId: false + name: true + numColumns: 1 + spinBox: true + slider: true + labelOnLeft: true + - + id: 8 + plugin: Body + class: LinkSelectionView + mounted: true + state: + listingMode: "Link List" + - + id: 10 + plugin: Python + class: PythonConsoleView + mounted: true +toolbars: + "TimeBar": + minTime: 0 + maxTime: 12000 + frameRate: 1000 + playbackFrameRate: 50 + idleLoopDrivenMode: false + currentTime: 0 + speedScale: 1 + syncToOngoingUpdates: true + autoExpansion: true + "KinematicsBar": + mode: AUTO + enablePositionDragger: true + penetrationBlock: false + collisionLinkHighlight: false + snapDistance: 0.025 + penetrationBlockDepth: 0.0005 + lazyCollisionDetectionMode: true + "LeggedBodyBar": + stanceWidth: 0.15 + "BodyMotionGenerationBar": + autoGenerationForNewBody: true + balancer: false + autoGeneration: false + timeScaleRatio: 1 + preInitialDuration: 1 + postFinalDuration: 1 + onlyTimeBarRange: false + makeNewBodyItem: true + stealthyStepMode: true + stealthyHeightRatioThresh: 2 + flatLiftingHeight: 0.005 + flatLandingHeight: 0.005 + impactReductionHeight: 0.005 + impactReductionTime: 0.04 + autoZmp: true + minZmpTransitionTime: 0.1 + zmpCenteringTimeThresh: 0.03 + zmpTimeMarginBeforeLiftingSpin: 0 + zmpMaxDistanceFromCenter: 0.02 + allLinkPositions: false + lipSyncMix: false + timeToStartBalancer: 0 + balancerIterations: 2 + plainBalancerMode: false + boundaryConditionType: position + boundarySmootherType: quintic + boundarySmootherTime: 0.5 + boundaryCmAdjustment: false + boundaryCmAdjustmentTime: 1 + waistHeightRelaxation: false + gravity: 9.8 + dynamicsTimeRatio: 1 +Body: + "BodyMotionEngine": + updateJointVelocities: false + "EditableSceneBody": + editableSceneBodies: + - + bodyItem: 2 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 3 + showCenterOfMass: false + showPpcom: false + showZmp: false + - + bodyItem: 4 + showCenterOfMass: false + showPpcom: false + showZmp: false + staticModelEditing: false + "KinematicFaultChecker": + checkJointPositions: true + angleMargin: 0 + translationMargin: 0 + checkJointVelocities: true + velocityLimitRatio: 100 + targetJoints: all + checkSelfCollisions: true + onlyTimeBarRange: false +OpenRTM: + "deleteUnmanagedRTCsOnStartingSimulation": false +viewAreas: + - + type: embedded + tabs: true + contents: + type: splitter + orientation: horizontal + sizes: [ 310, 2195 ] + children: + - + type: splitter + orientation: vertical + sizes: [ 768, 767 ] + children: + - + type: pane + views: [ 2 ] + current: 2 + - + type: pane + views: [ 1, 8 ] + current: 1 + - + type: splitter + orientation: vertical + sizes: [ 1205, 330 ] + children: + - + type: splitter + orientation: horizontal + sizes: [ 449, 1740 ] + children: + - + type: pane + views: [ 6, 7, 0 ] + current: 6 + - + type: pane + views: [ 4 ] + current: 4 + - + type: pane + views: [ 3, 10 ] + current: 10 +layoutOfToolBars: + rows: + - + - { name: "FileBar", x: 0, priority: 0 } + - { name: "ScriptBar", x: 47, priority: 3 } + - { name: "TimeBar", x: 47, priority: 1 } + - { name: "SceneBar", x: 1455, priority: 2 } + - { name: "SimulationBar", x: 1464, priority: 0 } diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsk_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsk_choreonoid.launch new file mode 100644 index 00000000..9885bbfb --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsk_choreonoid.launch @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch new file mode 100644 index 00000000..9256e5a5 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch new file mode 100644 index 00000000..459980a4 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py new file mode 100755 index 00000000..967646a6 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/scripts/hrp2_rh_setup.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python + +from hrpsys_ros_bridge_tutorials.hrp2_hrpsys_config import * + +class JSKHRP2ChoreonoidHrpsysConfigurator(JSKHRP2HrpsysConfigurator): + def waitForRobotHardware(self, robotname="Robot"): + '''!@brief + Wait for RobotHardware is exists and activated. + + @param robotname str: name of RobotHardware component. + ''' + self.rh = None + timeout_count = 0 + # wait for simulator or RobotHardware setup which sometime takes a long time + while self.rh == None and timeout_count < 10: # <- time out limit + if timeout_count > 0: # do not sleep initial loop + time.sleep(1); + self.rh = rtm.findRTC("RobotHardware_choreonoid0") + print(self.configurator_name + "wait for %s : %s ( timeout %d < 10)" % ( robotname, self.rh, timeout_count)) + if self.rh and self.rh.isActive() == None: # just in case rh is not ready... + self.rh = None + timeout_count += 1 + + if not self.rh: + print(self.configurator_name + "Could not find RobotHardware_choreonoid0") + if self.ms: + print(self.configurator_name + "Candidates are .... " + str([x.name() for x in self.ms.get_components()])) + print(self.configurator_name + "Exitting.... " + robotname) + exit(1) + + print(self.configurator_name + "findComps -> RobotHardware_choreonoid0 : %s isActive? = %s " % (self.rh, self.rh.isActive())) + + # wait for simulator or RobotHardware setup which sometime takes a long time + self.rh_choreonoid = None + while self.rh_choreonoid == None and timeout_count < 10: # <- time out limit + if timeout_count > 0: # do not sleep initial loop + time.sleep(1); + self.rh_choreonoid = rtm.findRTC(robotname) + print(self.configurator_name + "wait for %s : %s ( timeout %d < 10)" % ( robotname, self.rh_choreonoid, timeout_count)) + if self.rh_choreonoid and self.rh_choreonoid.isActive() == None: # just in case rh is not ready... + self.rh_choreonoid = None + timeout_count += 1 + + if not self.rh_choreonoid: + print(self.configurator_name + "Could not find " + robotname) + if self.ms: + print(self.configurator_name + "Candidates are .... " + str([x.name() for x in self.ms.get_components()])) + print(self.configurator_name + "Exitting.... " + robotname) + exit(1) + + print(self.configurator_name + "findComps -> %s : %s isActive? = %s " % (robotname, self.rh_choreonoid, self.rh_choreonoid.isActive())) + + def activateComps(self): + stash_rh = self.rh + self.rh = self.rh_choreonoid + HrpsysConfigurator.activateComps(self) + self.rh = stash_rh + + def startABSTIMP (self): + ### not used on hrpsys + if self.ROBOT_NAME == "HRP2JSKNT" or self.ROBOT_NAME == "HRP2JSKNTS": + self.el_svc.setServoErrorLimit("RARM_JOINT7", sys.float_info.max) + self.el_svc.setServoErrorLimit("LARM_JOINT7", sys.float_info.max) + self.rh_svc.servo("RARM_JOINT7",OpenHRP.RobotHardwareService.SWITCH_OFF) + self.rh_svc.servo("LARM_JOINT7",OpenHRP.RobotHardwareService.SWITCH_OFF) + self.rh_svc.setServoGainPercentage("RLEG_JOINT6", 30.0) + self.rh_svc.setServoGainPercentage("LLEG_JOINT6", 30.0) + ### + self.startAutoBalancer() + # Suppress limit over message and behave like real robot that always angle-vector is in seq. + # Latter four 0.0 are for hands. + self.seq_svc.setJointAngles(self.hrp2ResetPose(), 1.0) + self.ic_svc.startImpedanceController("larm") + self.ic_svc.startImpedanceController("rarm") + self.startStabilizer() + +if __name__ == '__main__': + if os.environ["CHOREONOID_ROBOT"] == "HRP2JSKNTS": + robot_name = "HRP2JSKNTS" + elif os.environ["CHOREONOID_ROBOT"] == "HRP2JSKNT": + robot_name = "HRP2JSKNT" + elif os.environ["CHOREONOID_ROBOT"] == "HRP2JSK": + robot_name = "HRP2JSK" + else: + robot_name = "" + hcf = JSKHRP2ChoreonoidHrpsysConfigurator(robot_name) + if len(sys.argv) > 2 : + hcf.init(sys.argv[1], sys.argv[2]) + hcf.startABSTIMP() + elif len(sys.argv) > 1 : + hcf.init(sys.argv[1]) + hcf.startABSTIMP() + else : + hcf.init()