From ea1e0f0c2d8197e02d31cc23354c20c6f0e930a7 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 13 Jun 2021 11:12:55 +0900 Subject: [PATCH 1/4] [hrpsys_choreonoid/iob.cpp] remove assumption for force sensor --- hrpsys_choreonoid/iob/iob.cpp | 89 ++++++++++++++++------------------- 1 file changed, 40 insertions(+), 49 deletions(-) diff --git a/hrpsys_choreonoid/iob/iob.cpp b/hrpsys_choreonoid/iob/iob.cpp index cd82b9a3..0edbdd59 100644 --- a/hrpsys_choreonoid/iob/iob.cpp +++ b/hrpsys_choreonoid/iob/iob.cpp @@ -14,6 +14,8 @@ #include #include #include +#include +#include #include "RobotHardware_choreonoid.h" @@ -66,19 +68,13 @@ static OutPort *op_torqueOut; //* *// static TimedDoubleSeq m_qvel_sim; static TimedDoubleSeq m_torque_sim; -static TimedDoubleSeq m_rfsensor_sim; -static TimedDoubleSeq m_lfsensor_sim; -static TimedDoubleSeq m_rhsensor_sim; -static TimedDoubleSeq m_lhsensor_sim; +static std::vector m_forcesensor_sim; static TimedAcceleration3D m_gsensor_sim; static TimedAngularVelocity3D m_gyrometer_sim; static InPort *ip_qvel_sim; static InPort *ip_torque_sim; -static InPort *ip_rfsensor_sim; -static InPort *ip_lfsensor_sim; -static InPort *ip_rhsensor_sim; -static InPort *ip_lhsensor_sim; +static std::vector* > ip_forcesensor_sim; static InPort *ip_gsensor_sim; static InPort *ip_gyrometer_sim; @@ -578,6 +574,27 @@ int open_iob(void) std::cerr << ", iob_step = " << iob_step << std::endl; } + // load robot + hrp::BodyPtr m_robot; + { + RTC::Manager& rtcManager = RTC::Manager::instance(); + std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; + int comPos = nameServer.find(","); + if (comPos < 0){ + comPos = nameServer.length(); + } + nameServer = nameServer.substr(0, comPos); + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); + RTC::Properties& prop = self_ptr->getProperties(); + m_robot = hrp::BodyPtr(new hrp::Body()); + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), + CosNaming::NamingContext::_duplicate(naming.getRootContext()) + )){ + std::cerr << "failed to load model[" << prop["model"] << "]" << std::endl; + return RTC::RTC_ERROR; + } + } + //* for PD controller *// ip_angleIn = new InPort ("angleIn", m_angleIn); op_torqueOut = new OutPort ("torqueOut", m_torqueOut); @@ -588,19 +605,18 @@ int open_iob(void) //* pass through port *// ip_qvel_sim = new InPort ("qvel_sim", m_qvel_sim); ip_torque_sim = new InPort ("torque_sim", m_torque_sim); - ip_rfsensor_sim = new InPort ("rfsensor_sim", m_rfsensor_sim); - ip_lfsensor_sim = new InPort ("lfsensor_sim", m_lfsensor_sim); - ip_rhsensor_sim = new InPort ("rhsensor_sim", m_rhsensor_sim); - ip_lhsensor_sim = new InPort ("lhsensor_sim", m_lhsensor_sim); + m_forcesensor_sim.resize(m_robot->numSensors(hrp::Sensor::FORCE)); + for (unsigned int i=0; i < m_robot->numSensors(hrp::Sensor::FORCE); i++){ + ip_forcesensor_sim.push_back(new InPort ((m_robot->sensor(hrp::Sensor::FORCE, i)->name + "_sim").c_str(), m_forcesensor_sim[i])); + } ip_gsensor_sim = new InPort ("gsensor_sim", m_gsensor_sim); ip_gyrometer_sim = new InPort ("gyrometer_sim", m_gyrometer_sim); self_ptr->addInPort("qvel_sim", *ip_qvel_sim); self_ptr->addInPort("torque_sim", *ip_torque_sim); - self_ptr->addInPort("rfsensor_sim", *ip_rfsensor_sim); - self_ptr->addInPort("lfsensor_sim", *ip_lfsensor_sim); - self_ptr->addInPort("rhsensor_sim", *ip_rhsensor_sim); - self_ptr->addInPort("lhsensor_sim", *ip_lhsensor_sim); + for (unsigned int i=0; i < m_robot->numSensors(hrp::Sensor::FORCE); i++){ + self_ptr->addInPort((m_robot->sensor(hrp::Sensor::FORCE, i)->name + "_sim").c_str(), *(ip_forcesensor_sim[i])); + } self_ptr->addInPort("gsensor_sim", *ip_gsensor_sim); self_ptr->addInPort("gyrometer_sim", *ip_gyrometer_sim); @@ -651,39 +667,14 @@ void iob_update(void) } } //* *// - if(ip_rfsensor_sim->isNew()) { - ip_rfsensor_sim->read(); - if(number_of_force_sensors() >= 1) { - for(int i = 0; i < 6; i++) { - //forces[0][i] = m_rfsensor_sim.data[i]; - force_queue[force_counter][0][i] = m_rfsensor_sim.data[i]; - } - } - } - if(ip_lfsensor_sim->isNew()) { - ip_lfsensor_sim->read(); - if(number_of_force_sensors() >= 2) { - for(int i = 0; i < 6; i++) { - //forces[1][i] = m_lfsensor_sim.data[i]; - force_queue[force_counter][1][i] = m_lfsensor_sim.data[i]; - } - } - } - if(ip_rhsensor_sim->isNew()) { - ip_rhsensor_sim->read(); - if(number_of_force_sensors() >= 3) { - for(int i = 0; i < 6; i++) { - //forces[2][i] = m_rhsensor_sim.data[i]; - force_queue[force_counter][2][i] = m_rhsensor_sim.data[i]; - } - } - } - if(ip_lhsensor_sim->isNew()) { - ip_lhsensor_sim->read(); - if(number_of_force_sensors() >= 4) { - for(int i = 0; i < 6; i++) { - //forces[3][i] = m_lhsensor_sim.data[i]; - force_queue[force_counter][3][i] = m_lhsensor_sim.data[i]; + for (unsigned int i=0; i < ip_forcesensor_sim.size(); i++) { + if(ip_forcesensor_sim[i]->isNew()) { + ip_forcesensor_sim[i]->read(); + if(number_of_force_sensors() > i) { + for(int j = 0; j < 6; j++) { + //forces[j][i] = ip_forcesensor_sim[j].data[i]; + force_queue[force_counter][i][j] = m_forcesensor_sim[i].data[j]; + } } } } From 253eecad9b30da088975980b51ea53564c6c1cfd Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 13 Jun 2021 11:51:37 +0900 Subject: [PATCH 2/4] [hrpsys_choreonoid/JAXONCustomizer.cpp] remove assumption for bush --- hrpsys_choreonoid/src/JAXONCustomizer.cpp | 109 ++++++++++++---------- 1 file changed, 59 insertions(+), 50 deletions(-) diff --git a/hrpsys_choreonoid/src/JAXONCustomizer.cpp b/hrpsys_choreonoid/src/JAXONCustomizer.cpp index 09944c64..9d702322 100644 --- a/hrpsys_choreonoid/src/JAXONCustomizer.cpp +++ b/hrpsys_choreonoid/src/JAXONCustomizer.cpp @@ -2,6 +2,8 @@ #include #include +#include +#include #include #include #include @@ -56,18 +58,15 @@ struct JointValSet double* valuePtr; double* velocityPtr; double* torqueForcePtr; + double spring; + double damping; }; struct JAXONCustomizer { BodyHandle bodyHandle; - bool hasVirtualBushJoints; - JointValSet jointValSets[2][3]; - double springT; - double dampingT; - double springR; - double dampingR; + std::vector jointValSets; double tilt_upper_bound; double tilt_lower_bound; @@ -101,30 +100,55 @@ static const char** getTargetModelNames() return names; } -static void getVirtualbushJoints(JAXONCustomizer* customizer, BodyHandle body) +static void getVirtualbushJoints(JAXONCustomizer* customizer, BodyHandle body, const YAML::Node& param) { - customizer->hasVirtualBushJoints = true; + double default_springT = 1.1e6; // N/m + double default_dampingT = 1.1e3; // N/(m/s) + double default_springR = 2.5e3; // Nm / rad + double default_dampingR = 2.5; // Nm / (rad/s) + if (param && param["bush"]) { + if (param["bush"]["springT"]) default_springT = param["bush"]["springT"].as(); + if (param["bush"]["dampingT"]) default_dampingT = param["bush"]["dampingT"].as(); + if (param["bush"]["springR"]) default_springR = param["bush"]["springR"].as(); + if (param["bush"]["dampingR"]) default_dampingR = param["bush"]["dampingR"].as(); + } - int bushIndices[2][3]; + std::vector bushes{"RLEG", "LLEG", "RARM", "LARM"}; + if (param && param["bush"] && param["bush"]["bushes"]) bushes = param["bush"]["bushes"].as>(); + std::vector types{"X", "Y", "Z", "ROLL", "PITCH", "YAW"}; - 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 < bushes.size(); ++i){ + for(int j=0; j < types.size(); ++j){ + std::string bush_name = bushes[i] + "_BUSH_" + types[j]; + int bushIndex = bodyInterface->getLinkIndexFromName(body, bush_name.c_str()); - 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]; + std::cerr << "[Customizer] failed to find out : " << bush_name << std::endl; + }else{ + std::cerr << "[Customizer] find out : " << bush_name << std::endl; + + JointValSet jointValSet; jointValSet.valuePtr = bodyInterface->getJointValuePtr(body, bushIndex); jointValSet.velocityPtr = bodyInterface->getJointVelocityPtr(body, bushIndex); jointValSet.torqueForcePtr = bodyInterface->getJointForcePtr(body, bushIndex); + + if (j<3) { + jointValSet.spring = default_springT; + jointValSet.damping = default_dampingT; + if (param && param["bush"] && param["bush"][bushes[i]]){ + if (param["bush"][bushes[i]]["springT"]) jointValSet.spring = param["bush"][bushes[i]]["springT"].as(); + if (param["bush"][bushes[i]]["dampingT"]) jointValSet.damping = param["bush"][bushes[i]]["dampingT"].as(); + } + } else { + jointValSet.spring = default_springR; + jointValSet.damping = default_dampingR; + if (param && param["bush"] && param["bush"][bushes[i]]){ + if (param["bush"][bushes[i]]["springR"]) jointValSet.spring = param["bush"][bushes[i]]["springR"].as(); + if (param["bush"][bushes[i]]["dampingR"]) jointValSet.damping = param["bush"][bushes[i]]["dampingR"].as(); + } + } + + customizer->jointValSets.push_back(jointValSet); } } } @@ -166,38 +190,31 @@ static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName) customizer = new JAXONCustomizer; customizer->bodyHandle = bodyHandle; - customizer->hasVirtualBushJoints = false; - - customizer->springT = 1.1e6; // N/m - customizer->dampingT = 1.1e3; // N/(m/s) - customizer->springR = 2.5e3; // Nm / rad - customizer->dampingR = 2.5; // Nm / (rad/s) customizer->tilt_upper_bound = 1.35; // rad customizer->tilt_lower_bound = -0.7; // rad customizer->tilt_positive_speed = 1.0; // rad/s customizer->tilt_negative_speed = -1.0; // rad/s + YAML::Node param; char* config_file_path = getenv("CUSTOMIZER_CONF_FILE"); if (config_file_path) { ifstream ifs(config_file_path); if (ifs.is_open()) { std::cerr << "[JAXONCustomizer] Config file is: " << config_file_path << std::endl; - YAML::Node param = YAML::LoadFile(config_file_path); - customizer->springT = param["bush"]["springT"].as(); - customizer->dampingT = param["bush"]["dampingT"].as(); - customizer->springR = param["bush"]["springR"].as(); - customizer->dampingR = param["bush"]["dampingR"].as(); - customizer->tilt_upper_bound = param["tilt_laser"]["TILT_UPPER_BOUND"].as(); - customizer->tilt_positive_speed = param["tilt_laser"]["TILT_POSITIVE_SPEED"].as(); - customizer->tilt_lower_bound = param["tilt_laser"]["TILT_LOWER_BOUND"].as(); - customizer->tilt_negative_speed = param["tilt_laser"]["TILT_NEGATIVE_SPEED"].as(); + param = YAML::LoadFile(config_file_path); + if (param["tilt_laser"]) { + if (param["tilt_laser"]["TILT_UPPER_BOUND"]) customizer->tilt_upper_bound = param["tilt_laser"]["TILT_UPPER_BOUND"].as(); + if (param["tilt_laser"]["TILT_POSITIVE_SPEED"]) customizer->tilt_positive_speed = param["tilt_laser"]["TILT_POSITIVE_SPEED"].as(); + if (param["tilt_laser"]["TILT_LOWER_BOUND"]) customizer->tilt_lower_bound = param["tilt_laser"]["TILT_LOWER_BOUND"].as(); + if (param["tilt_laser"]["TILT_NEGATIVE_SPEED"]) customizer->tilt_negative_speed = param["tilt_laser"]["TILT_NEGATIVE_SPEED"].as(); + } } else { std::cerr << "[JAXONCustomizer] " << config_file_path << " is not found" << std::endl; } } - getVirtualbushJoints(customizer, bodyHandle); + getVirtualbushJoints(customizer, bodyHandle, param); return static_cast(customizer); } @@ -215,18 +232,10 @@ 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 j=1; j < 3; ++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; - } - } + for(int i=0; i < customizer->jointValSets.size(); ++i){ + JointValSet& bush = customizer->jointValSets[i]; + *(bush.torqueForcePtr) = - bush.spring * (*bush.valuePtr) - bush.damping * (*bush.velocityPtr); + //std::cerr << i << " " << 0 << " " << *(bush.torqueForcePtr) << " = " << -bush.spring << " x " << *bush.valuePtr << " + " << - bush.damping << " x " << *bush.velocityPtr << std::endl; } if(customizer->hasMultisenseJoint) { From 3cd3da75571cf5cd3e9d819727937241696ad467 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 13 Jun 2021 12:33:23 +0900 Subject: [PATCH 3/4] [hrpsys_choreonoid] add SampleRobot --- README.md | 28 +- hrpsys_choreonoid/CMakeLists.txt | 10 + .../config/BodyRTC_SampleRobot.RH.conf | 35 ++ .../config/SampleRobotCustomizer.yaml | 5 + .../config/SampleRobot_RH_LOAD_OBJ.cnoid.in | 368 ++++++++++++++++++ hrpsys_choreonoid/config/flat.yaml | 5 + .../launch/samplerobot_choreonoid.launch.in | 41 ++ hrpsys_choreonoid/package.xml | 1 + .../scripts/samplerobot_rh_setup.py | 77 ++++ 9 files changed, 568 insertions(+), 2 deletions(-) create mode 100644 hrpsys_choreonoid/config/BodyRTC_SampleRobot.RH.conf create mode 100644 hrpsys_choreonoid/config/SampleRobotCustomizer.yaml create mode 100644 hrpsys_choreonoid/config/SampleRobot_RH_LOAD_OBJ.cnoid.in create mode 100644 hrpsys_choreonoid/config/flat.yaml create mode 100644 hrpsys_choreonoid/launch/samplerobot_choreonoid.launch.in create mode 100755 hrpsys_choreonoid/scripts/samplerobot_rh_setup.py diff --git a/README.md b/README.md index bf273922..dbdbcb9a 100644 --- a/README.md +++ b/README.md @@ -43,9 +43,35 @@ source devel/setup.bash ``` ### run test +If you get error, try `export ORBgiopMaxMsgSize=2097152000`. + +#### SampleRobot +``` +rtmlaunch hrpsys_choreonoid samplerobot_choreonoid.launch +``` +Launch another terminal and send command to robot. (python) +``` +ipython -i `rospack find hrpsys_choreonoid`/scripts/samplerobot_rh_setup.py "SampleRobot(Robot)0" +hcf.abc_svc.goPos(1,0,0) +``` +Launch another terminal and send command to robot. (euslisp) +``` +roseus `rospack find hrpsys_ros_bridge`/euslisp/samplerobot-interface.l +(samplerobot-init) +(send *ri* :go-pos 1 0 0) +(setq *robot* *sr*) +(objects *robot*) +(send *robot* :reset-manip-pose) +(send *ri* :angle-vector (send *robot* :angle-vector) 5000) +``` + +#### JAXON +Build hrpsys_choreonoid_tutorials ``` catkin build hrpsys_choreonoid_tutorials source devel/setup.bash +``` +``` rtmlaunch hrpsys_choreonoid_tutorials jaxon_jvrc_choreonoid.launch ``` Launch another terminal and send command to robot. (python) @@ -66,8 +92,6 @@ roseus `rospack find hrpsys_choreonoid_tutorials`/euslisp/jaxon_jvrc-interface.l (send *ri* :angle-vector (send *robot* :angle-vector) 5000) ``` -If you get error, try `export ORBgiopMaxMsgSize=2097152000`. - See also [hrpsys_choreonoid_tutorials/README.md](/hrpsys_choreonoid_tutorials/README.md) --- diff --git a/hrpsys_choreonoid/CMakeLists.txt b/hrpsys_choreonoid/CMakeLists.txt index 2571f847..ea696636 100644 --- a/hrpsys_choreonoid/CMakeLists.txt +++ b/hrpsys_choreonoid/CMakeLists.txt @@ -96,3 +96,13 @@ endif() add_subdirectory(iob) add_custom_target(hrpsys_choreonoid_iob ALL DEPENDS RobotHardware_choreonoid) + + +set(HRPSYS_CHOREONOID_DIRECTORY ${PROJECT_SOURCE_DIR}) +set(OPENHRP_SAMPLE_DIR ${openhrp3_PREFIX}/share/OpenHRP-3.1/sample) + +### +#SampleRobot conid +### +configure_file(${PROJECT_SOURCE_DIR}/config/SampleRobot_RH_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/SampleRobot_RH_LOAD_OBJ.cnoid @ONLY) +configure_file(${PROJECT_SOURCE_DIR}/launch/samplerobot_choreonoid.launch.in ${PROJECT_SOURCE_DIR}/launch/samplerobot_choreonoid.launch @ONLY) #OPENHRP_SAMPLE_DIR is difficult to find in roslaunch api diff --git a/hrpsys_choreonoid/config/BodyRTC_SampleRobot.RH.conf b/hrpsys_choreonoid/config/BodyRTC_SampleRobot.RH.conf new file mode 100644 index 00000000..240e4e57 --- /dev/null +++ b/hrpsys_choreonoid/config/BodyRTC_SampleRobot.RH.conf @@ -0,0 +1,35 @@ +## +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 +#### +# 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 diff --git a/hrpsys_choreonoid/config/SampleRobotCustomizer.yaml b/hrpsys_choreonoid/config/SampleRobotCustomizer.yaml new file mode 100644 index 00000000..8437b048 --- /dev/null +++ b/hrpsys_choreonoid/config/SampleRobotCustomizer.yaml @@ -0,0 +1,5 @@ +bush: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 2.5e3 + dampingR: 2.5 diff --git a/hrpsys_choreonoid/config/SampleRobot_RH_LOAD_OBJ.cnoid.in b/hrpsys_choreonoid/config/SampleRobot_RH_LOAD_OBJ.cnoid.in new file mode 100644 index 00000000..41d3bbde --- /dev/null +++ b/hrpsys_choreonoid/config/SampleRobot_RH_LOAD_OBJ.cnoid.in @@ -0,0 +1,368 @@ +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: "SampleRobot" + plugin: Body + class: BodyItem + data: + modelFile: "@OPENHRP_SAMPLE_DIR@/model/sample1_bush.wrl" + currentBaseLink: "WAIST" + rootPosition: [ 0.0, 0.0, 0.680 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ + -7.778932e-05, -0.378613, -0.00021, 0.832039, -0.452564, 0.000245, + 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, + -7.778932e-05, -0.378613, -0.00021, 0.832039, -0.452564, 0.000245, + 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0] + initialRootPosition: [ 0.0, 0.0, 0.680 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ + -7.778932e-05, -0.378613, -0.00021, 0.832039, -0.452564, 0.000245, + 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, + -7.778932e-05, -0.378613, -0.00021, 0.832039, -0.452564, 0.000245, + 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + children: + - + id: 3 + name: "BodyRTC" + plugin: OpenRTM + class: BodyRTCItem + data: + isImmediateMode: true + moduleName: "@HRPSYS_CHOREONOID_DIRECTORY@/lib/RobotHardware_choreonoid" + confFileName: "@HRPSYS_CHOREONOID_DIRECTORY@/config/BodyRTC_SampleRobot.RH.conf" + configurationMode: Use Configuration File + AutoConnect: false + InstanceName: SampleRobot(Robot)0 + bodyPeriodicRate: 0.002 + - + id: 4 + name: "add_objects.py" + plugin: Python + class: PythonScriptItem + data: + file: "@HRPSYS_CHOREONOID_DIRECTORY@/launch/add_objects.py" + executionOnLoading: true + backgroundExecution: false + - + 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/config/flat.yaml b/hrpsys_choreonoid/config/flat.yaml new file mode 100644 index 00000000..9ba61a95 --- /dev/null +++ b/hrpsys_choreonoid/config/flat.yaml @@ -0,0 +1,5 @@ +obj1: + name: 'VISIBLE_FLOOR' + file: '$(find choreonoid)/share/model/misc/floor.body' + translation: [0, 0, -0.1] + rotation: [[1, 0, 0], [0, 1, 0], [0, 0, 1]] diff --git a/hrpsys_choreonoid/launch/samplerobot_choreonoid.launch.in b/hrpsys_choreonoid/launch/samplerobot_choreonoid.launch.in new file mode 100644 index 00000000..ed643fff --- /dev/null +++ b/hrpsys_choreonoid/launch/samplerobot_choreonoid.launch.in @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid/package.xml b/hrpsys_choreonoid/package.xml index 9fc908cc..2012d558 100644 --- a/hrpsys_choreonoid/package.xml +++ b/hrpsys_choreonoid/package.xml @@ -24,6 +24,7 @@ hrpsys openhrp3 choreonoid + hrpsys_ros_bridge roscpp tf openrtm_aist diff --git a/hrpsys_choreonoid/scripts/samplerobot_rh_setup.py b/hrpsys_choreonoid/scripts/samplerobot_rh_setup.py new file mode 100755 index 00000000..e1bd02ba --- /dev/null +++ b/hrpsys_choreonoid/scripts/samplerobot_rh_setup.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python + +import sys +import rospkg +sys.path.append(rospkg.RosPack().get_path("hrpsys_ros_bridge") + "/test") +from samplerobot_hrpsys_config import * + +class SampleRobotChoreonoidHrpsysConfigurator(SampleRobotHrpsysConfigurator): + 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): + self.startAutoBalancer() + self.ic_svc.startImpedanceController("larm") + self.ic_svc.startImpedanceController("rarm") + self.startStabilizer() + +if __name__ == '__main__': + hcf = SampleRobotChoreonoidHrpsysConfigurator("SampleRobot") + 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() From b8dc97b5ea19db81fb270a2a5160f381461affef Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 13 Jun 2021 14:34:06 +0900 Subject: [PATCH 4/4] [hrpsys_choreonoid_tutorials] add HRP2JSK* --- hrpsys_choreonoid_tutorials/CMakeLists.txt | 29 ++ .../config/BodyRTC_HRP2JSK.RH.conf | 34 ++ .../config/BodyRTC_HRP2JSKNT.RH.conf | 34 ++ .../config/BodyRTC_HRP2JSKNTS.RH.conf | 39 ++ .../config/HRP2JSKCustomizer.yaml | 5 + .../config/HRP2JSKNTCustomizer.yaml | 21 + .../config/HRP2JSKNTSCustomizer.yaml | 21 + .../config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in | 413 ++++++++++++++++++ .../config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in | 372 ++++++++++++++++ .../config/HRP2JSK_RH_LOAD_OBJ.cnoid.in | 368 ++++++++++++++++ .../launch/hrp2jsk_choreonoid.launch | 48 ++ .../launch/hrp2jsknt_choreonoid.launch | 48 ++ .../launch/hrp2jsknts_choreonoid.launch | 61 +++ .../launch/hrp2jsknts_vision_connect.launch | 83 ++++ .../scripts/hrp2jsk_rh_setup.py | 14 + .../scripts/hrp2jsknt_rh_setup.py | 14 + .../scripts/hrp2jsknts_rh_setup.py | 14 + .../hrp2_choreonoid_hrpsys_config.py | 78 ++++ 18 files changed, 1696 insertions(+) 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/HRP2JSKCustomizer.yaml create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKNTCustomizer.yaml create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKNTSCustomizer.yaml create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in create mode 100644 hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_LOAD_OBJ.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 100644 hrpsys_choreonoid_tutorials/launch/hrp2jsknts_vision_connect.launch create mode 100755 hrpsys_choreonoid_tutorials/scripts/hrp2jsk_rh_setup.py create mode 100755 hrpsys_choreonoid_tutorials/scripts/hrp2jsknt_rh_setup.py create mode 100755 hrpsys_choreonoid_tutorials/scripts/hrp2jsknts_rh_setup.py create mode 100755 hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/hrp2_choreonoid_hrpsys_config.py diff --git a/hrpsys_choreonoid_tutorials/CMakeLists.txt b/hrpsys_choreonoid_tutorials/CMakeLists.txt index 159ab4ab..9e2b2581 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. @@ -198,6 +206,27 @@ configure_file(${PROJECT_SOURCE_DIR}/config/TABLIS_RH_FLAT.cnoid.in ${PROJECT_SO configure_file(${PROJECT_SOURCE_DIR}/config/TABLIS_BASE_RH_FLAT.cnoid.in ${PROJECT_SOURCE_DIR}/config/TABLIS_BASE_RH_FLAT.cnoid @ONLY) endif() +### +#HRP2JSK conid +### +if (${hrp2_models_FOUND}) +configure_file(${PROJECT_SOURCE_DIR}/config/HRP2JSK_RH_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/HRP2JSK_RH_LOAD_OBJ.cnoid @ONLY) +endif() + +### +#HRP2JSKNT conid +### +if (${hrp2_models_FOUND}) +configure_file(${PROJECT_SOURCE_DIR}/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid @ONLY) +endif() + +### +#HRP2JSKNTS conid +### +if (${hrp2_models_FOUND}) +configure_file(${PROJECT_SOURCE_DIR}/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/HRP2JSKNTS_RH_LOAD_OBJ.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..af0e57a7 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSK.RH.conf @@ -0,0 +1,34 @@ +## +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 +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 +#### +# 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 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..af0e57a7 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNT.RH.conf @@ -0,0 +1,34 @@ +## +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 +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 +#### +# 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 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..d2a25ba9 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/BodyRTC_HRP2JSKNTS.RH.conf @@ -0,0 +1,39 @@ +## +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 +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 +#### +# 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_DEPTH:CARMINE_DEPTH:CAMERA_RANGE +out-port = HEAD_CAMERA:CARMINE:CAMERA_IMAGE diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKCustomizer.yaml b/hrpsys_choreonoid_tutorials/config/HRP2JSKCustomizer.yaml new file mode 100644 index 00000000..f548fd68 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKCustomizer.yaml @@ -0,0 +1,5 @@ +bush: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 1.0e3 + dampingR: 1.0 diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKNTCustomizer.yaml b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTCustomizer.yaml new file mode 100644 index 00000000..eab5bf75 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTCustomizer.yaml @@ -0,0 +1,21 @@ +bush: + RLEG: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 1.0e3 + dampingR: 1.0 + LLEG: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 1.0e3 + dampingR: 1.0 + RARM: + springT: 1.0e5 + dampingT: 1.0e3 + springR: 3.0e2 + dampingR: 3.0 + LARM: + springT: 1.0e5 + dampingT: 1.0e3 + springR: 3.0e2 + dampingR: 3.0 diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKNTSCustomizer.yaml b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTSCustomizer.yaml new file mode 100644 index 00000000..eab5bf75 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTSCustomizer.yaml @@ -0,0 +1,21 @@ +bush: + RLEG: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 1.0e3 + dampingR: 1.0 + LLEG: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 1.0e3 + dampingR: 1.0 + RARM: + springT: 1.0e5 + dampingT: 1.0e3 + springR: 3.0e2 + dampingR: 3.0 + LARM: + springT: 1.0e5 + dampingT: 1.0e3 + springR: 3.0e2 + dampingR: 3.0 diff --git a/hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in new file mode 100644 index 00000000..e85cdf87 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKNTS_RH_LOAD_OBJ.cnoid.in @@ -0,0 +1,413 @@ +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: "HRP2JSKNTS" + 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.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 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.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.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.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 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.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.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: "add_objects.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/launch/add_objects.py" + executionOnLoading: true + backgroundExecution: false + - + 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 + children: + - + id: 7 + name: "GLVisionSimulator" + plugin: Body + class: GLVisionSimulatorItem + data: + enabled: true + targetBodies: [ HRP2JSKNTS ] + targetSensors: [ CARMINE ] + maxFrameRate: 1000 + maxLatency: 1 + recordVisionData: false + useThread: true + useThreadsForSensors: true + bestEffort: false + allSceneObjects: false + rangeSensorPrecisionRatio: 2 + depthError: 0 + enableHeadLight: true + enableAdditionalLights: true + - + id: 8 + name: "GLVisionSimulator" + plugin: Body + class: GLVisionSimulatorItem + data: + enabled: true + targetBodies: [ HRP2JSKNTS ] + targetSensors: [ CARMINE_DEPTH ] + maxFrameRate: 1000 + maxLatency: 1 + recordVisionData: false + useThread: true + useThreadsForSensors: true + bestEffort: false + allSceneObjects: false + rangeSensorPrecisionRatio: 2 + depthError: 0 + enableHeadLight: true + enableAdditionalLights: true + - + 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_LOAD_OBJ.cnoid.in b/hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in new file mode 100644 index 00000000..f63647ce --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSKNT_RH_LOAD_OBJ.cnoid.in @@ -0,0 +1,372 @@ +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: "HRP2JSKNT" + 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.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 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.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.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.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 0.000000, + 0.000366, 0.002938, -0.383414, 0.868598, -0.485184, -0.002938, 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.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.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: "add_objects.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/launch/add_objects.py" + executionOnLoading: true + backgroundExecution: false + - + 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_LOAD_OBJ.cnoid.in b/hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_LOAD_OBJ.cnoid.in new file mode 100644 index 00000000..f934e54f --- /dev/null +++ b/hrpsys_choreonoid_tutorials/config/HRP2JSK_RH_LOAD_OBJ.cnoid.in @@ -0,0 +1,368 @@ +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: "add_objects.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/launch/add_objects.py" + executionOnLoading: true + backgroundExecution: false + - + 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..b9c50776 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsk_choreonoid.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch new file mode 100644 index 00000000..b72719c9 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsknt_choreonoid.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch new file mode 100644 index 00000000..07f68010 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_choreonoid.launch @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + footcoords: + invert_odom_init: false + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_vision_connect.launch b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_vision_connect.launch new file mode 100644 index 00000000..b7d55db5 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/launch/hrp2jsknts_vision_connect.launch @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + [500, 0, 319.5, 0, 500, 239.5, 0, 0, 1] + [500, 0, 319.5, 0, 0, 500, 239.5, 0, 0, 0, 1, 0] + + + + + + + + + + + + + + + + + + + + + filter_field_name: z + filter_limit_min: 0.5 # depth camera cannot see too close area. `frontClipDistance` cannot be used for this purpose because camera sees through within `frontClipDistance`. + filter_limit_max: 10.0 + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid_tutorials/scripts/hrp2jsk_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/hrp2jsk_rh_setup.py new file mode 100755 index 00000000..4219a43c --- /dev/null +++ b/hrpsys_choreonoid_tutorials/scripts/hrp2jsk_rh_setup.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +from hrpsys_choreonoid_tutorials.hrp2_choreonoid_hrpsys_config import * + +if __name__ == '__main__': + hcf = JSKHRP2ChoreonoidHrpsysConfigurator("HRP2JSK") + 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() diff --git a/hrpsys_choreonoid_tutorials/scripts/hrp2jsknt_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/hrp2jsknt_rh_setup.py new file mode 100755 index 00000000..47613892 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/scripts/hrp2jsknt_rh_setup.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +from hrpsys_choreonoid_tutorials.hrp2_choreonoid_hrpsys_config import * + +if __name__ == '__main__': + hcf = JSKHRP2ChoreonoidHrpsysConfigurator("HRP2JSKNT") + 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() diff --git a/hrpsys_choreonoid_tutorials/scripts/hrp2jsknts_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/hrp2jsknts_rh_setup.py new file mode 100755 index 00000000..8bca2513 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/scripts/hrp2jsknts_rh_setup.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +from hrpsys_choreonoid_tutorials.hrp2_choreonoid_hrpsys_config import * + +if __name__ == '__main__': + hcf = JSKHRP2ChoreonoidHrpsysConfigurator("HRP2JSKNTS") + 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() diff --git a/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/hrp2_choreonoid_hrpsys_config.py b/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/hrp2_choreonoid_hrpsys_config.py new file mode 100755 index 00000000..733cdf04 --- /dev/null +++ b/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/hrp2_choreonoid_hrpsys_config.py @@ -0,0 +1,78 @@ +#!/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): + 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) + for j in dict(self.Groups)["rhand"] + dict(self.Groups)["lhand"]: + self.el_svc.setServoErrorLimit(j, sys.float_info.max) + self.rh_svc.setServoErrorLimit(j, 0.0) + + self.startAutoBalancer() + self.seq_svc.setJointAngles(self.hrp2ResetPose(), 1.0) + if self.ROBOT_NAME == "HRP2JSKNT" or self.ROBOT_NAME == "HRP2JSKNTS": + self.seq_svc.setJointAnglesOfGroup("rhand", [0, 0, 0, 0, 0, 0], 1.0) + self.seq_svc.setJointAnglesOfGroup("lhand", [0, 0, 0, 0, 0, 0], 1.0) + self.ic_svc.startImpedanceController("larm") + self.ic_svc.startImpedanceController("rarm") + self.startStabilizer()