void rotate_body(int direction, double length){ double move_x = -length*cos(degree_to_rad(direction))/2.0; double move_y = length*sin(degree_to_rad(direction))/2.0; double move_x1 = -length*cos(degree_to_rad(180 - direction))/2.0; double move_y1 = length*sin(degree_to_rad(180 - direction))/2.0; int i,a; int y_multi = 1; uint16_t max = 0; for(i = 1; i <= 6; ++i){ a = current_step%10; if(i == 1 || i == 4 || i == 5){ a = (current_step + 5)%10; } y_multi = (i % 2 == 0 ? 1 : -1); if(i % 2 == 1){ max = get_max_2(ik(step_multi[a][0]*move_x, y_multi*step_multi[a][1]*move_y, step_multi[a][2]*LIFT_HEIGHT, i), max); }else if(i % 2 == 0){ max = get_max_2(ik(step_multi[a][0]*move_x1, y_multi*step_multi[a][1]*move_y1, step_multi[a][2]*LIFT_HEIGHT, i), max); } } current_step++; SERVO_ACTION; robot_delay2(max); }
static klassOop load_and_initialize_klass(Symbol* sh, TRAPS) { klassOop k = SystemDictionary::resolve_or_fail(sh, true, CHECK_NULL); instanceKlassHandle ik (THREAD, k); if (ik->should_be_initialized()) { ik->initialize(CHECK_NULL); } return ik(); }
void Controller::reach() { // Change to reaching pose Eigen::VectorXd newPose; Eigen::Vector3d position; mDesiredDofs = mDefaultPose; mDesiredDofs[6] = 0.2; mDesiredDofs[9] = 0.2; mDesiredDofs[14] = -0.2; mDesiredDofs[15] = -0.2; mDesiredDofs[17] = -0.2; mDesiredDofs[19] = -0.2; mDesiredDofs[27] = 0.7; mDesiredDofs[28] = -2.5; mDesiredDofs[30] = 0.7; mDesiredDofs[31] = 2.5; mDesiredDofs[33] = 0.4; mDesiredDofs[34] = 0.4; if(numBars == 1){ position = barPositions[0]; } else{ position = barPositions[1]; } position.z() = -1.0 * (handDiffZ/2.0); newPose = ik(mSkel->getBodyNode("h_hand_left"), position); mDesiredDofs += (newPose - mSkel->getPositions()); position.z() = 1.0 * (handDiffZ/2.0); newPose = ik(mSkel->getBodyNode("h_hand_right"), position); mDesiredDofs += (newPose - mSkel->getPositions()); stablePD(); checkContactState(); if (mFootContact) { // If feet are in contact again, go back to JUMP and continue to push mState = "JUMP"; std::cout << mCurrentFrame << ": " << "REACH -> JUMP" << std::endl; } else if (mLeftHandContact || mRightHandContact) { mState = "GRAB"; mTimer = 500; std::cout << mCurrentFrame << ": " << "REACH -> GRAB" << std::endl; } else { mState = "REACH"; } }
/** * Main function */ int main() { try { OrientationIK ik( "futureOrientationInverseKinematics.osim", "futureOrientationInverseKinematics.trc", "futureOrientationInverseKinematics", "humerus_r", "radius_r"); ik.run(); } catch (const std::exception& ex) { std::cout << "Exception: " << ex.what() << std::endl; return 1; } catch (...) { std::cout << "Unrecognized exception " << std::endl; return 1; } //system("pause"); return 0; }
uint8_t init_body(int direction, double length){ double move_x = -length*cos(degree_to_rad(direction))/2.0; double move_y = length*sin(degree_to_rad(direction))/2.0; int i,a; int y_multi = 1; uint16_t max = 0; for(i = 1; i <= 6; ++i){ a = current_step%10; if(current_step%10 == 5){ current_step = 0; a = 0; } if(a != 0 && (i == 1 || i == 4 || i == 5)){ a = (current_step + 5)%10; } y_multi = (i % 2 == 0 ? 1 : -1); max = get_max_2(ik(step_multi[a][0]*move_x, y_multi*step_multi[a][1]*move_y, step_multi[a][2]*LIFT_HEIGHT, i), max); } if(current_step % 10 != 0) current_step++; SERVO_ACTION; robot_delay2(max); return current_step%10; }
void JMXStatusDCmd::execute(DCmdSource source, TRAPS) { ResourceMark rm(THREAD); HandleMark hm(THREAD); // Load and initialize the sun.management.Agent class // invoke getManagementAgentStatus() method to generate the status info // throw java.lang.NoSuchMethodError if method doesn't exist Handle loader = Handle(THREAD, SystemDictionary::java_system_loader()); Klass* k = SystemDictionary::resolve_or_fail(vmSymbols::sun_management_Agent(), loader, Handle(), true, CHECK); instanceKlassHandle ik (THREAD, k); JavaValue result(T_OBJECT); JavaCalls::call_static(&result, ik, vmSymbols::getAgentStatus_name(), vmSymbols::void_string_signature(), CHECK); jvalue* jv = (jvalue*) result.get_value_addr(); oop str = (oop) jv->l; if (str != NULL) { char* out = java_lang_String::as_utf8_string(str); if (out) { output()->print_cr("%s", out); return; } } output()->print_cr("Error obtaining management agent status"); }
Handle ThreadStackTrace::allocate_fill_stack_trace_element_array(TRAPS) { Klass* k = SystemDictionary::StackTraceElement_klass(); assert(k != NULL, "must be loaded in 1.4+"); instanceKlassHandle ik(THREAD, k); // Allocate an array of java/lang/StackTraceElement object objArrayOop ste = oopFactory::new_objArray(ik(), _depth, CHECK_NH); objArrayHandle backtrace(THREAD, ste); for (int j = 0; j < _depth; j++) { StackFrameInfo* frame = _frames->at(j); methodHandle mh(THREAD, frame->method()); oop element = java_lang_StackTraceElement::create(mh, frame->bci(), CHECK_NH); backtrace->obj_at_put(j, element); } return backtrace; }
int ArmAnalyticalInverseKinematics::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, std::vector<KDL::JntArray>& q_out) { KDL::JntArray solution; bool bools[] = { true, false }; // there are no solutions available yet q_out.clear(); // iterate over all redundant solutions for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { solution = ik(p_in, bools[i], bools[j]); if (isSolutionValid(solution)) q_out.push_back(solution); } } if (q_out.size() > 0) { ROS_DEBUG("Inverse Kinematic found a solution"); return 1; } else { ROS_DEBUG("Inverse Kinematic found no solution."); return -1; } }
DLLIMPORT int estimate_qps_xyz11A(double *qps, double *kinArray, double *nlArray, double *jntArray, double *xyz, double *fmin) { int ret = -1; FwdK11A fk11a(kinArray2Struct11A(kinArray)); InvK_nlopt<TaskXYZ<FwdK11A>, FwdK11A> ik(fk11a, jntArray2Struct(jntArray), nlArray2Struct(nlArray)); ret = ik.solve(qps, xyz, fmin); return ret; }
GSSCredential::GSSCredential(const std::string& proxyPath, const std::string& certificatePath, const std::string& keyPath) : credential(GSS_C_NO_CREDENTIAL) { std::string credbuf; if (!proxyPath.empty()) { std::ifstream is(proxyPath.c_str()); getline(is, credbuf, '\0'); if(!is || credbuf.empty()) { logger.msg(ERROR, "Failed to read proxy file: %s", proxyPath); return; } } else if (!certificatePath.empty() && !keyPath.empty()) { std::ifstream is(certificatePath.c_str()); getline(is, credbuf, '\0'); if(!is || credbuf.empty()) { logger.msg(ERROR, "Failed to read certificate file: %s", certificatePath); return; } std::string keybuf; std::ifstream ik(keyPath.c_str()); getline(ik, keybuf, '\0'); if(!ik || keybuf.empty()) { logger.msg(ERROR, "Failed to read private key file: %s", keyPath); return; } credbuf += "\n"; credbuf += keybuf; } if(!credbuf.empty()) { //Convert to GSS credental only if find credential content OM_uint32 majstat, minstat; gss_buffer_desc gbuf; gbuf.value = (void*)credbuf.c_str(); gbuf.length = credbuf.length(); majstat = gss_import_cred(&minstat, &credential, NULL, 0, &gbuf, GSS_C_INDEFINITE, NULL); if (GSS_ERROR(majstat)) { credential = GSS_C_NO_CREDENTIAL; logger.msg(ERROR, "Failed to convert GSI credential to " "GSS credential (major: %d, minor: %d)%s", majstat, minstat, ErrorStr(majstat, minstat)); return; } } }
DLLIMPORT int estimate_qps_xyzuxuyuz11A(double *qps, double *kinArray, double *nlArray, double *jntArray, double *xyz, double *uxyz, double *fmin) { int ret = -1; FwdK11A fk11a(kinArray2Struct11A(kinArray)); InvK_nlopt<TaskXYZUxUyUz<FwdK11A>, FwdK11A> ik(fk11a, jntArray2Struct(jntArray), nlArray2Struct(nlArray)); double x[6] = { xyz[0],xyz[1],xyz[2], uxyz[0],uxyz[1],uxyz[2] }; ret = ik.solve(qps, x, fmin); xyz[0] = x[0]; xyz[1] = x[1]; xyz[2] = x[2]; uxyz[0] = x[3]; uxyz[1] = x[4]; uxyz[2] = x[5]; return ret; }
void JMXStartRemoteDCmd::execute(TRAPS) { ResourceMark rm(THREAD); HandleMark hm(THREAD); // Load and initialize the sun.management.Agent class // invoke startRemoteManagementAgent(string) method to start // the remote management server. // throw java.lang.NoSuchMethodError if the method doesn't exist Handle loader = Handle(THREAD, SystemDictionary::java_system_loader()); klassOop k = SystemDictionary::resolve_or_fail(vmSymbols::sun_management_Agent(), loader, Handle(), true, CHECK); instanceKlassHandle ik (THREAD, k); JavaValue result(T_VOID); // Pass all command line arguments to java as key=value,... // All checks are done on java side int len = 0; stringStream options; char comma[2] = {0,0}; // Leave default values on Agent.class side and pass only // agruments explicitly set by user. All arguments passed // to jcmd override properties with the same name set by // command line with -D or by managmenent.properties // file. #define PUT_OPTION(a) \ if ( (a).is_set() ){ \ options.print("%scom.sun.management.%s=%s", comma, (a).name(), (a).value()); \ comma[0] = ','; \ } PUT_OPTION(_config_file); PUT_OPTION(_jmxremote_port); PUT_OPTION(_jmxremote_rmi_port); PUT_OPTION(_jmxremote_ssl); PUT_OPTION(_jmxremote_registry_ssl); PUT_OPTION(_jmxremote_authenticate); PUT_OPTION(_jmxremote_password_file); PUT_OPTION(_jmxremote_access_file); PUT_OPTION(_jmxremote_login_config); PUT_OPTION(_jmxremote_ssl_enabled_cipher_suites); PUT_OPTION(_jmxremote_ssl_enabled_protocols); PUT_OPTION(_jmxremote_ssl_need_client_auth); PUT_OPTION(_jmxremote_ssl_config_file); #undef PUT_OPTION Handle str = java_lang_String::create_from_str(options.as_string(), CHECK); JavaCalls::call_static(&result, ik, vmSymbols::startRemoteAgent_name(), vmSymbols::string_void_signature(), str, CHECK); }
// Dump stack trace of threads specified in the given threads array. // Returns StackTraceElement[][] each element is the stack trace of a thread in // the corresponding entry in the given threads array Handle ThreadService::dump_stack_traces(GrowableArray<instanceHandle>* threads, int num_threads, TRAPS) { assert(num_threads > 0, "just checking"); ThreadDumpResult dump_result; VM_ThreadDump op(&dump_result, threads, num_threads, -1, /* entire stack */ false, /* with locked monitors */ false /* with locked synchronizers */); VMThread::execute(&op); // Allocate the resulting StackTraceElement[][] object ResourceMark rm(THREAD); klassOop k = SystemDictionary::resolve_or_fail(vmSymbols::java_lang_StackTraceElement_array(), true, CHECK_NH); objArrayKlassHandle ik (THREAD, k); objArrayOop r = oopFactory::new_objArray(ik(), num_threads, CHECK_NH); objArrayHandle result_obj(THREAD, r); int num_snapshots = dump_result.num_snapshots(); assert(num_snapshots == num_threads, "Must have num_threads thread snapshots"); int i = 0; for (ThreadSnapshot* ts = dump_result.snapshots(); ts != NULL; i++, ts = ts->next()) { ThreadStackTrace* stacktrace = ts->get_stack_trace(); if (stacktrace == NULL) { // No stack trace result_obj->obj_at_put(i, NULL); } else { // Construct an array of java/lang/StackTraceElement object Handle backtrace_h = stacktrace->allocate_fill_stack_trace_element_array(CHECK_NH); result_obj->obj_at_put(i, backtrace_h()); } } return result_obj; }
main() { // A study is the top level component that contains the model and other // computational components. Study inverseKinematicsStudy; Model model("subject_01.osim"); inverseKinematicsStudy.addComponent(model); // A data Source component wraps a TimeSeriesTables and access rows by time // Each column is given its own Output by name unless specified otherwise Source markers("subject01_trial2.trc"); // Source markers("subject01_trial2.trc", Vec2(0, 5)); // by range // Source markers("subject01_trial2.trc", {"aa, "ai", ac"}); //by name inverseKinematicsStudy.addComponent(markers); // InverseKinematicsSolver is wrapped by a component (or one itself?) // dependency on model, and marker inputs (outputs of a source) wired // upon connect InverseKinematics ik(model, markers); inverseKinematicsStudy.addComponent(ik); // Now handle IK Outputs // Extract the outputs from the model and the state via a reporter // Do not need to know type BUT cannot combine different types // Coordinates are doubles and modelMarkers are Vec3's OutputReporter coordinateReporter(); OutputReporter modelMarkerReporter(); // Output coordinates from IK for(const auto& coord: mode.getCoordinates()) coordinateReporter.addOutput(coord.getOutput("value")); inverseKinematicsStudy.addComponent(coordinateReporter); // Output model marker locations from IK for (const auto& marker : mode.getMarkers()) modelMarkerReporter.addOutputs(marker.getOutput("location")); inverseKinematicsStudy.addComponent(modelMarkerReporter); State& state = inverseKinematicsStudy.initSystem(); // March through time to solve for the state's coordinates for (double t : markers.getTimes()) { state.setTime(t); ik.track(state); inverseKinematicsStudy.realizeReport(state); } // write results to file FileAdapter fileAdapter; fileAdapter.write("s01_tr2_IK.mot", coordinateReporter.getReport()); fileAdapter.write("s01_tr2_markers.trc", modelMarkerReporter.getReport()); }
QString KExiv2::getIptcTagDescription(const char* iptcTagName) { try { std::string iptckey(iptcTagName); Exiv2::IptcKey ik(iptckey); return QString::fromLocal8Bit( Exiv2::IptcDataSets::dataSetDesc(ik.tag(), ik.record()) ); } catch (Exiv2::Error& e) { d->printExiv2ExceptionError("Cannot get metadata tag description using Exiv2 ", e); } return QString(); }
void JMXStopRemoteDCmd::execute(DCmdSource source, TRAPS) { ResourceMark rm(THREAD); HandleMark hm(THREAD); // Load and initialize the sun.management.Agent class // invoke stopRemoteManagementAgent method to stop the // management server // throw java.lang.NoSuchMethodError if method doesn't exist Handle loader = Handle(THREAD, SystemDictionary::java_system_loader()); Klass* k = SystemDictionary::resolve_or_fail(vmSymbols::sun_management_Agent(), loader, Handle(), true, CHECK); instanceKlassHandle ik (THREAD, k); JavaValue result(T_VOID); JavaCalls::call_static(&result, ik, vmSymbols::stopRemoteAgent_name(), vmSymbols::void_method_signature(), CHECK); }
static jint get_properties(AttachOperation* op, outputStream* out, Symbol* serializePropertiesMethod) { Thread* THREAD = Thread::current(); HandleMark hm; // load sun.misc.VMSupport Symbol* klass = vmSymbols::sun_misc_VMSupport(); klassOop k = load_and_initialize_klass(klass, THREAD); if (HAS_PENDING_EXCEPTION) { java_lang_Throwable::print(PENDING_EXCEPTION, out); CLEAR_PENDING_EXCEPTION; return JNI_ERR; } instanceKlassHandle ik(THREAD, k); // invoke the serializePropertiesToByteArray method JavaValue result(T_OBJECT); JavaCallArguments args; Symbol* signature = vmSymbols::serializePropertiesToByteArray_signature(); JavaCalls::call_static(&result, ik, serializePropertiesMethod, signature, &args, THREAD); if (HAS_PENDING_EXCEPTION) { java_lang_Throwable::print(PENDING_EXCEPTION, out); CLEAR_PENDING_EXCEPTION; return JNI_ERR; } // The result should be a [B oop res = (oop)result.get_jobject(); assert(res->is_typeArray(), "just checking"); assert(typeArrayKlass::cast(res->klass())->element_type() == T_BYTE, "just checking"); // copy the bytes to the output stream typeArrayOop ba = typeArrayOop(res); jbyte* addr = typeArrayOop(res)->byte_at_addr(0); out->print_raw((const char*)addr, ba->length()); return JNI_OK; }
void PrintSystemPropertiesDCmd::execute(DCmdSource source, TRAPS) { // load VMSupport Symbol* klass = vmSymbols::jdk_internal_vm_VMSupport(); Klass* k = SystemDictionary::resolve_or_fail(klass, true, CHECK); instanceKlassHandle ik (THREAD, k); if (ik->should_be_initialized()) { ik->initialize(THREAD); } if (HAS_PENDING_EXCEPTION) { java_lang_Throwable::print(PENDING_EXCEPTION, output()); output()->cr(); CLEAR_PENDING_EXCEPTION; return; } // invoke the serializePropertiesToByteArray method JavaValue result(T_OBJECT); JavaCallArguments args; Symbol* signature = vmSymbols::serializePropertiesToByteArray_signature(); JavaCalls::call_static(&result, ik, vmSymbols::serializePropertiesToByteArray_name(), signature, &args, THREAD); if (HAS_PENDING_EXCEPTION) { java_lang_Throwable::print(PENDING_EXCEPTION, output()); output()->cr(); CLEAR_PENDING_EXCEPTION; return; } // The result should be a [B oop res = (oop)result.get_jobject(); assert(res->is_typeArray(), "just checking"); assert(TypeArrayKlass::cast(res->klass())->element_type() == T_BYTE, "just checking"); // copy the bytes to the output stream typeArrayOop ba = typeArrayOop(res); jbyte* addr = typeArrayOop(res)->byte_at_addr(0); output()->print_raw((const char*)addr, ba->length()); }
Handle MemoryService::create_MemoryUsage_obj(MemoryUsage usage, TRAPS) { Klass* k = Management::java_lang_management_MemoryUsage_klass(CHECK_NH); instanceKlassHandle ik(THREAD, k); instanceHandle obj = ik->allocate_instance_handle(CHECK_NH); JavaValue result(T_VOID); JavaCallArguments args(10); args.push_oop(obj); // receiver args.push_long(usage.init_size_as_jlong()); // Argument 1 args.push_long(usage.used_as_jlong()); // Argument 2 args.push_long(usage.committed_as_jlong()); // Argument 3 args.push_long(usage.max_size_as_jlong()); // Argument 4 JavaCalls::call_special(&result, ik, vmSymbols::object_initializer_name(), vmSymbols::long_long_long_long_void_signature(), &args, CHECK_NH); return obj; }
oop LiveFrameStream::create_primitive_value_instance(StackValueCollection* values, int i, TRAPS) { Klass* k = SystemDictionary::resolve_or_null(vmSymbols::java_lang_LiveStackFrameInfo(), CHECK_NULL); instanceKlassHandle ik (THREAD, k); JavaValue result(T_OBJECT); JavaCallArguments args; Symbol* signature = NULL; // ## TODO: type is only available in LocalVariable table, if present. // ## StackValue type is T_INT or T_OBJECT. switch (values->at(i)->type()) { case T_INT: args.push_int(values->int_at(i)); signature = vmSymbols::asPrimitive_int_signature(); break; case T_LONG: args.push_long(values->long_at(i)); signature = vmSymbols::asPrimitive_long_signature(); break; case T_FLOAT: args.push_float(values->float_at(i)); signature = vmSymbols::asPrimitive_float_signature(); break; case T_DOUBLE: args.push_double(values->double_at(i)); signature = vmSymbols::asPrimitive_double_signature(); break; case T_BYTE: args.push_int(values->int_at(i)); signature = vmSymbols::asPrimitive_byte_signature(); break; case T_SHORT: args.push_int(values->int_at(i)); signature = vmSymbols::asPrimitive_short_signature(); break; case T_CHAR: args.push_int(values->int_at(i)); signature = vmSymbols::asPrimitive_char_signature(); break; case T_BOOLEAN: args.push_int(values->int_at(i)); signature = vmSymbols::asPrimitive_boolean_signature(); break; case T_OBJECT: return values->obj_at(i)(); case T_CONFLICT: // put a non-null slot args.push_int(0); signature = vmSymbols::asPrimitive_int_signature(); break; default: ShouldNotReachHere(); } JavaCalls::call_static(&result, ik, vmSymbols::asPrimitive_name(), signature, &args, CHECK_NULL); return (instanceOop) result.get_jobject(); }
static Handle createGcInfo(GCMemoryManager *gcManager, GCStatInfo *gcStatInfo,TRAPS) { // Fill the arrays of MemoryUsage objects with before and after GC // per pool memory usage Klass* mu_klass = Management::java_lang_management_MemoryUsage_klass(CHECK_NH); instanceKlassHandle mu_kh(THREAD, mu_klass); // The array allocations below should use a handle containing mu_klass // as the first allocation could trigger a GC, causing the actual // klass oop to move, and leaving mu_klass pointing to the old // location. objArrayOop bu = oopFactory::new_objArray(mu_kh(), MemoryService::num_memory_pools(), CHECK_NH); objArrayHandle usage_before_gc_ah(THREAD, bu); objArrayOop au = oopFactory::new_objArray(mu_kh(), MemoryService::num_memory_pools(), CHECK_NH); objArrayHandle usage_after_gc_ah(THREAD, au); for (int i = 0; i < MemoryService::num_memory_pools(); i++) { Handle before_usage = MemoryService::create_MemoryUsage_obj(gcStatInfo->before_gc_usage_for_pool(i), CHECK_NH); Handle after_usage; MemoryUsage u = gcStatInfo->after_gc_usage_for_pool(i); if (u.max_size() == 0 && u.used() > 0) { // If max size == 0, this pool is a survivor space. // Set max size = -1 since the pools will be swapped after GC. MemoryUsage usage(u.init_size(), u.used(), u.committed(), (size_t)-1); after_usage = MemoryService::create_MemoryUsage_obj(usage, CHECK_NH); } else { after_usage = MemoryService::create_MemoryUsage_obj(u, CHECK_NH); } usage_before_gc_ah->obj_at_put(i, before_usage()); usage_after_gc_ah->obj_at_put(i, after_usage()); } // Current implementation only has 1 attribute (number of GC threads) // The type is 'I' objArrayOop extra_args_array = oopFactory::new_objArray(SystemDictionary::Integer_klass(), 1, CHECK_NH); objArrayHandle extra_array (THREAD, extra_args_array); Klass* itKlass = SystemDictionary::Integer_klass(); instanceKlassHandle intK(THREAD, itKlass); instanceHandle extra_arg_val = intK->allocate_instance_handle(CHECK_NH); { JavaValue res(T_VOID); JavaCallArguments argsInt; argsInt.push_oop(extra_arg_val); argsInt.push_int(gcManager->num_gc_threads()); JavaCalls::call_special(&res, intK, vmSymbols::object_initializer_name(), vmSymbols::int_void_signature(), &argsInt, CHECK_NH); } extra_array->obj_at_put(0,extra_arg_val()); Klass* gcInfoklass = Management::com_sun_management_GcInfo_klass(CHECK_NH); instanceKlassHandle ik(THREAD, gcInfoklass); Handle gcInfo_instance = ik->allocate_instance_handle(CHECK_NH); JavaValue constructor_result(T_VOID); JavaCallArguments constructor_args(16); constructor_args.push_oop(gcInfo_instance); constructor_args.push_oop(getGcInfoBuilder(gcManager,THREAD)); constructor_args.push_long(gcStatInfo->gc_index()); constructor_args.push_long(Management::ticks_to_ms(gcStatInfo->start_time())); constructor_args.push_long(Management::ticks_to_ms(gcStatInfo->end_time())); constructor_args.push_oop(usage_before_gc_ah); constructor_args.push_oop(usage_after_gc_ah); constructor_args.push_oop(extra_array); JavaCalls::call_special(&constructor_result, ik, vmSymbols::object_initializer_name(), vmSymbols::com_sun_management_GcInfo_constructor_signature(), &constructor_args, CHECK_NH); return Handle(gcInfo_instance()); }
int main(int argc, char** argv) { IKREAL_TYPE eerot[9],eetrans[3]; #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_joints = GetNumJoints(); unsigned int num_free_parameters = GetNumFreeParameters(); #else // for IKFast 54 unsigned int num_of_joints = getNumJoints(); unsigned int num_free_parameters = getNumFreeParameters(); #endif std::string cmd; if (argv[1]) cmd = argv[1]; //printf("command: %s \n\n", cmd.c_str() ); if (cmd.compare("ik") == 0) // ik mode { if( argc == 1+7+num_free_parameters+1 ) // ik, given translation vector and quaternion pose { #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); eetrans[0] = atof(argv[2]); eetrans[1] = atof(argv[3]); eetrans[2] = atof(argv[4]); // Convert input effector pose, in w x y z quaternion notation, to rotation matrix. // Must use doubles, else lose precision compared to directly inputting the rotation matrix. double qw = atof(argv[5]); double qx = atof(argv[6]); double qy = atof(argv[7]); double qz = atof(argv[8]); const double n = 1.0f/sqrt(qx*qx+qy*qy+qz*qz+qw*qw); qw *= n; qx *= n; qy *= n; qz *= n; eerot[0] = 1.0f - 2.0f*qy*qy - 2.0f*qz*qz; eerot[1] = 2.0f*qx*qy - 2.0f*qz*qw; eerot[2] = 2.0f*qx*qz + 2.0f*qy*qw; eerot[3] = 2.0f*qx*qy + 2.0f*qz*qw; eerot[4] = 1.0f - 2.0f*qx*qx - 2.0f*qz*qz; eerot[5] = 2.0f*qy*qz - 2.0f*qx*qw; eerot[6] = 2.0f*qx*qz - 2.0f*qy*qw; eerot[7] = 2.0f*qy*qz + 2.0f*qx*qw; eerot[8] = 1.0f - 2.0f*qx*qx - 2.0f*qy*qy; // For debugging, output the matrix for (unsigned char i=0; i<=8; i++) { // detect -0.0 and replace with 0.0 if ( ((int&)(eerot[i]) & 0xFFFFFFFF) == 0) eerot[i] = 0.0; } printf(" Rotation %f %f %f \n", eerot[0], eerot[1], eerot[2] ); printf(" %f %f %f \n", eerot[3], eerot[4], eerot[5] ); printf(" %f %f %f \n", eerot[6], eerot[7], eerot[8] ); printf("\n"); for(std::size_t i = 0; i < vfree.size(); ++i) vfree[i] = atof(argv[13+i]); #if IK_VERSION > 54 // for IKFast 56,61 bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); #else // for IKFast 54 bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif if( !bSuccess ) { fprintf(stderr,"Failed to get ik solution\n"); //return -1; } #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); std::vector<IKREAL_TYPE> solvalues(num_of_joints); for(std::size_t i = 0; i < num_of_solutions; ++i) { #if IK_VERSION > 54 // for IKFast 56,61 const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i); int this_sol_free_params = (int)sol.GetFree().size(); #else // for IKFast 54 int this_sol_free_params = (int)vsolutions[i].GetFree().size(); #endif printf("sol%d (free=%d): ", (int)i, this_sol_free_params ); std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params); #if IK_VERSION > 54 // for IKFast 56,61 sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #else // for IKFast 54 vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #endif for( std::size_t j = 0; j < solvalues.size(); ++j) printf("%.15f, ", solvalues[j]); printf("\n"); } } else if( argc == 1+12+num_free_parameters+1 ) // ik, given rotation-translation matrix { #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); eerot[0] = atof(argv[2]); eerot[1] = atof(argv[3]); eerot[2] = atof(argv[4]); eetrans[0] = atof(argv[5]); eerot[3] = atof(argv[6]); eerot[4] = atof(argv[7]); eerot[5] = atof(argv[8]); eetrans[1] = atof(argv[9]); eerot[6] = atof(argv[10]); eerot[7] = atof(argv[11]); eerot[8] = atof(argv[12]); eetrans[2] = atof(argv[13]); for(std::size_t i = 0; i < vfree.size(); ++i) vfree[i] = atof(argv[14+i]); printf("translation: \n"); for (unsigned int i = 0; i < 3; i++) { printf("%lf ", eetrans[i]); } printf("\n"); printf("rotation matix: \n"); for (unsigned int i = 0; i < 9; i++) { printf("%lf%s", eerot[i], (i % 3 == 2)?"\n":" "); } printf("\n"); printf("vfree:\n"); for(std::size_t i = 0; i < vfree.size(); ++i) printf("%lf ", vfree[i]); printf("\n"); #if IK_VERSION > 54 // for IKFast 56,61 bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); #else // for IKFast 54 bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif if( !bSuccess ) { fprintf(stderr,"Failed to get ik solution\n"); return -1; } #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); std::vector<IKREAL_TYPE> solvalues(num_of_joints); for(std::size_t i = 0; i < num_of_solutions; ++i) { #if IK_VERSION > 54 // for IKFast 56,61 const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i); int this_sol_free_params = (int)sol.GetFree().size(); #else // for IKFast 54 int this_sol_free_params = (int)vsolutions[i].GetFree().size(); #endif printf("sol%d (free=%d): ", (int)i, this_sol_free_params ); std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params); #if IK_VERSION > 54 // for IKFast 56,61 sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #else // for IKFast 54 vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #endif for( std::size_t j = 0; j < solvalues.size(); ++j) printf("%.15f, ", solvalues[j]); printf("\n"); } } else { printf("\n " "Usage: \n\n " " ./compute ik t0 t1 t2 qw qi qj qk free0 ...\n\n " " Returns the ik solutions given the transformation of the end effector specified by \n " " a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n " " There are %d free parameters that have to be specified.\n\n", num_free_parameters ); printf(" \n " " ./compute ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n " " Returns the ik solutions given the transformation of the end effector specified by \n " " a 3x3 rotation R (rXX), and a 3x1 translation (tX). \n " " There are %d free parameters that have to be specified.\n\n", num_free_parameters ); return 1; } } // endif ik mode else if (cmd.compare("fk") == 0) // fk mode { if( argc != num_of_joints+2 ) { printf("\n " "Usage: \n\n " " ./compute fk j0 j1 ... j%d \n\n" " Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 ); return 1; } printf("\n\n"); // Put input joint values into array IKREAL_TYPE joints[num_of_joints]; for (unsigned int i=0; i<num_of_joints; i++) { joints[i] = atof(argv[i+2]); } #if IK_VERSION > 54 // for IKFast 56,61 ComputeFk(joints, eetrans, eerot); // void return #else // for IKFast 54 fk(joints, eetrans, eerot); // void return #endif printf("Found fk solution for end frame: \n\n"); printf(" Translation: x: %f y: %f z: %f \n", eetrans[0], eetrans[1], eetrans[2] ); printf("\n"); printf(" Rotation %f %f %f \n", eerot[0], eerot[1], eerot[2] ); printf(" Matrix: %f %f %f \n", eerot[3], eerot[4], eerot[5] ); printf(" %f %f %f \n", eerot[6], eerot[7], eerot[8] ); printf("\n"); // Display equivalent Euler angles float yaw; float pitch; float roll; if ( eerot[5] > 0.998 || eerot[5] < -0.998 ) { // singularity yaw = IKatan2( -eerot[6], eerot[0] ); pitch = 0; } else { yaw = IKatan2( eerot[2], eerot[8] ); pitch = IKatan2( eerot[3], eerot[4] ); } roll = IKasin( eerot[5] ); printf(" Euler angles: \n"); printf(" Yaw: %f ", yaw ); printf("(1st: rotation around vertical blue Z-axis in ROS Rviz) \n"); printf(" Pitch: %f \n", pitch ); printf(" Roll: %f \n", roll ); printf("\n"); // Convert rotation matrix to quaternion (Daisuke Miyazaki) float q0 = ( eerot[0] + eerot[4] + eerot[8] + 1.0f) / 4.0f; float q1 = ( eerot[0] - eerot[4] - eerot[8] + 1.0f) / 4.0f; float q2 = (-eerot[0] + eerot[4] - eerot[8] + 1.0f) / 4.0f; float q3 = (-eerot[0] - eerot[4] + eerot[8] + 1.0f) / 4.0f; if(q0 < 0.0f) q0 = 0.0f; if(q1 < 0.0f) q1 = 0.0f; if(q2 < 0.0f) q2 = 0.0f; if(q3 < 0.0f) q3 = 0.0f; q0 = sqrt(q0); q1 = sqrt(q1); q2 = sqrt(q2); q3 = sqrt(q3); if(q0 >= q1 && q0 >= q2 && q0 >= q3) { q0 *= +1.0f; q1 *= SIGN(eerot[7] - eerot[5]); q2 *= SIGN(eerot[2] - eerot[6]); q3 *= SIGN(eerot[3] - eerot[1]); } else if(q1 >= q0 && q1 >= q2 && q1 >= q3) { q0 *= SIGN(eerot[7] - eerot[5]); q1 *= +1.0f; q2 *= SIGN(eerot[3] + eerot[1]); q3 *= SIGN(eerot[2] + eerot[6]); } else if(q2 >= q0 && q2 >= q1 && q2 >= q3) { q0 *= SIGN(eerot[2] - eerot[6]); q1 *= SIGN(eerot[3] + eerot[1]); q2 *= +1.0f; q3 *= SIGN(eerot[7] + eerot[5]); } else if(q3 >= q0 && q3 >= q1 && q3 >= q2) { q0 *= SIGN(eerot[3] - eerot[1]); q1 *= SIGN(eerot[6] + eerot[2]); q2 *= SIGN(eerot[7] + eerot[5]); q3 *= +1.0f; } else { printf("Error while converting to quaternion! \n"); } float r = NORM(q0, q1, q2, q3); q0 /= r; q1 /= r; q2 /= r; q3 /= r; printf(" Quaternion: %f %f %f %f \n", q0, q1, q2, q3 ); printf(" "); // print quaternion with convention and +/- signs such that it can be copy-pasted into WolframAlpha.com printf("%f ", q0); if (q1 > 0) printf("+ %fi ", q1); else if (q1 < 0) printf("- %fi ", -q1); else printf("+ 0.00000i "); if (q2 > 0) printf("+ %fj ", q2); else if (q2 < 0) printf("- %fj ", -q2); else printf("+ 0.00000j "); if (q3 > 0) printf("+ %fk ", q3); else if (q3 < 0) printf("- %fk ", -q3); else printf("+ 0.00000k "); printf(" (alternate convention) \n"); printf("\n\n"); } else if (cmd.compare("iktiming") == 0) // generate random ik and check time performance { if( argc != 2 ) { printf("\n " "Usage: \n\n " " ./compute iktiming \n\n" " For fixed number of iterations, generates random joint angles, then \n" " calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 ); return 1; } printf("\n\n"); #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); //for(std::size_t i = 0; i < vfree.size(); ++i) // vfree[i] = atof(argv[13+i]); srand( (unsigned)time(0) ); // seed random number generator float min = -3.14; float max = 3.14; float rnd; IKREAL_TYPE joints[num_of_joints]; timespec start_time, end_time; unsigned int elapsed_time = 0; unsigned int sum_time = 0; unsigned int fail_count = 0; #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_tests = 10; #else // for IKFast 54 unsigned int num_of_tests = 100000; #endif for (unsigned int i=0; i < num_of_tests; i++) { // Measure avg time for whole process //clock_gettime(CLOCK_REALTIME, &start_time); // Put random joint values into array for (unsigned int i=0; i<num_of_joints; i++) { float rnd = (float)rand() / (float)RAND_MAX; joints[i] = min + rnd * (max - min); } /* printf("Joint angles: "); for (unsigned int i=0; i<num_of_joints; i++) { printf("%f ", joints[i] ); } printf("\n"); */ #if IK_VERSION > 54 // for IKFast 56,61 ComputeFk(joints, eetrans, eerot); // void return #else // for IKFast 54 fk(joints, eetrans, eerot); // void return #endif // Measure avg time for IK clock_gettime(CLOCK_REALTIME, &start_time); #if IK_VERSION > 54 // for IKFast 56,61 vfree[0] = 5; if (!ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions)) { fail_count++; } else { printf("joint angles: \n"); for (unsigned int i=0; i<num_of_joints; i++) { printf("%lf ", joints[i]); } printf("\n"); printf("translation: \n"); for (unsigned int i = 0; i < 3; i++) { printf("%lf ", eetrans[i]); } printf("\n"); printf("rotation matix: \n"); for (unsigned int i = 0; i < 9; i++) { printf("%lf%s", eerot[i], (i % 3 == 2)?"\n":" "); } printf("\n"); /*unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); printf("Found %d ik solutions:\n", num_of_solutions ); std::vector<IKREAL_TYPE> solvalues(num_of_joints); for(std::size_t i = 0; i < num_of_solutions; ++i) { const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i); int this_sol_free_params = (int)sol.GetFree().size(); printf("sol%d (free=%d): ", (int)i, this_sol_free_params ); std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params); sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); for( std::size_t j = 0; j < solvalues.size(); ++j) printf("%.15f, ", solvalues[j]); printf("\n"); }*/ } #else // for IKFast 54 ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif /* #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); */ clock_gettime(CLOCK_REALTIME, &end_time); elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec); sum_time += elapsed_time; } // endfor unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests; printf("avg time: %f ms over %d tests \n", (float)avg_time/1000.0, num_of_tests ); printf("Failed of %d tests\n", fail_count); return 1; } else if (cmd.compare("iktiming2") == 0) // for fixed joint values, check time performance of ik { if( argc != 2 ) { printf("\n " "Usage: \n\n " " ./compute iktiming2 \n\n" " For fixed number of iterations, with one set of joint variables, this \n" " finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 ); return 1; } printf("\n\n"); #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); //for(std::size_t i = 0; i < vfree.size(); ++i) // vfree[i] = atof(argv[13+i]); IKREAL_TYPE joints[num_of_joints]; timespec start_time, end_time; unsigned int elapsed_time = 0; unsigned int sum_time = 0; #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_tests = 1000000; #else // for IKFast 54 unsigned int num_of_tests = 100000; #endif // fixed rotation-translation matrix corresponding to an unusual robot pose eerot[0] = 0.002569; eerot[1] = -0.658044; eerot[2] = -0.752975; eetrans[0] = 0.121937; eerot[3] = 0.001347; eerot[4] = -0.752975; eerot[5] = 0.658048; eetrans[1] = -0.276022; eerot[6] = -0.999996; eerot[7] = -0.002705; eerot[8] = -0.001047; eetrans[2] = 0.005685; for (unsigned int i=0; i < num_of_tests; i++) { clock_gettime(CLOCK_REALTIME, &start_time); #if IK_VERSION > 54 // for IKFast 56,61 ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); #else // for IKFast 54 ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif /* #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); */ clock_gettime(CLOCK_REALTIME, &end_time); elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec); sum_time += elapsed_time; } // endfor unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests; printf("avg time: %f ms over %d tests \n", (float)avg_time/1000.0, num_of_tests ); return 1; } else { printf("\n" "Usage: \n\n"); printf(" ./compute fk j0 j1 ... j%d \n\n" " Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 ); printf("\n" " ./compute ik t0 t1 t2 qw qi qj qk free0 ... \n\n" " Returns the ik solutions given the transformation of the end effector specified by \n" " a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n" " There are %d free parameters that have to be specified. \n\n", num_free_parameters ); printf(" \n" " ./compute ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" " Returns the ik solutions given the transformation of the end effector specified by \n" " a 3x3 rotation R (rXX), and a 3x1 translation (tX). \n" " There are %d free parameters that have to be specified. \n\n", num_free_parameters ); printf("\n" " ./compute iktiming \n\n" " For fixed number of iterations, generates random joint angles, then \n" " calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 ); printf("\n" " ./compute iktiming2 \n\n" " For fixed number of iterations, with one set of joint variables, this \n" " finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 ); return 1; } return 0; }
instanceOop MemoryManager::get_memory_manager_instance(TRAPS) { // Must do an acquire so as to force ordering of subsequent // loads from anything _memory_mgr_obj points to or implies. instanceOop mgr_obj = (instanceOop)OrderAccess::load_ptr_acquire(&_memory_mgr_obj); if (mgr_obj == NULL) { // It's ok for more than one thread to execute the code up to the locked region. // Extra manager instances will just be gc'ed. klassOop k = Management::sun_management_ManagementFactory_klass(CHECK_0); instanceKlassHandle ik(THREAD, k); Handle mgr_name = java_lang_String::create_from_str(name(), CHECK_0); JavaValue result(T_OBJECT); JavaCallArguments args; args.push_oop(mgr_name); // Argument 1 Symbol* method_name = NULL; Symbol* signature = NULL; if (is_gc_memory_manager()) { method_name = vmSymbols::createGarbageCollector_name(); signature = vmSymbols::createGarbageCollector_signature(); args.push_oop(Handle()); // Argument 2 (for future extension) } else { method_name = vmSymbols::createMemoryManager_name(); signature = vmSymbols::createMemoryManager_signature(); } JavaCalls::call_static(&result, ik, method_name, signature, &args, CHECK_0); instanceOop m = (instanceOop) result.get_jobject(); instanceHandle mgr(THREAD, m); { // Get lock before setting _memory_mgr_obj // since another thread may have created the instance MutexLocker ml(Management_lock); // Check if another thread has created the management object. We reload // _memory_mgr_obj here because some other thread may have initialized // it while we were executing the code before the lock. // // The lock has done an acquire, so the load can't float above it, but // we need to do a load_acquire as above. mgr_obj = (instanceOop)OrderAccess::load_ptr_acquire(&_memory_mgr_obj); if (mgr_obj != NULL) { return mgr_obj; } // Get the address of the object we created via call_special. mgr_obj = mgr(); // Use store barrier to make sure the memory accesses associated // with creating the management object are visible before publishing // its address. The unlock will publish the store to _memory_mgr_obj // because it does a release first. OrderAccess::release_store_ptr(&_memory_mgr_obj, mgr_obj); } } return mgr_obj; }
// Revised lookup semantics introduced 1.3 (Kestral beta) void klassVtable::initialize_vtable(bool checkconstraints, TRAPS) { // Note: Arrays can have intermediate array supers. Use java_super to skip them. KlassHandle super (THREAD, klass()->java_super()); int nofNewEntries = 0; if (PrintVtables && !klass()->oop_is_array()) { ResourceMark rm(THREAD); tty->print_cr("Initializing: %s", _klass->name()->as_C_string()); } #ifdef ASSERT oop* end_of_obj = (oop*)_klass() + _klass()->size(); oop* end_of_vtable = (oop*)&table()[_length]; assert(end_of_vtable <= end_of_obj, "vtable extends beyond end"); #endif if (Universe::is_bootstrapping()) { // just clear everything for (int i = 0; i < _length; i++) table()[i].clear(); return; } int super_vtable_len = initialize_from_super(super); if (klass()->oop_is_array()) { assert(super_vtable_len == _length, "arrays shouldn't introduce new methods"); } else { assert(_klass->oop_is_instance(), "must be instanceKlass"); objArrayHandle methods(THREAD, ik()->methods()); int len = methods()->length(); int initialized = super_vtable_len; // update_inherited_vtable can stop for gc - ensure using handles for (int i = 0; i < len; i++) { HandleMark hm(THREAD); assert(methods()->obj_at(i)->is_method(), "must be a methodOop"); methodHandle mh(THREAD, (methodOop)methods()->obj_at(i)); bool needs_new_entry = update_inherited_vtable(ik(), mh, super_vtable_len, checkconstraints, CHECK); if (needs_new_entry) { put_method_at(mh(), initialized); mh()->set_vtable_index(initialized); // set primary vtable index initialized++; } } // add miranda methods; it will also update the value of initialized fill_in_mirandas(initialized); // In class hierarchies where the accessibility is not increasing (i.e., going from private -> // package_private -> publicprotected), the vtable might actually be smaller than our initial // calculation. assert(initialized <= _length, "vtable initialization failed"); for(;initialized < _length; initialized++) { put_method_at(NULL, initialized); } NOT_PRODUCT(verify(tty, true)); } }
void run() { // load model and data OpenSim::Model model(m_model_path); OpenSim::MarkerData imu_trc(m_imu_path); SimTK::State& state = model.initSystem(); // setup SimTK::Assembler ik(model.updMultibodySystem()); ik.setAccuracy(1e-5); SimTK::Markers* markers = new SimTK::Markers(); SimTK::OrientationSensors* imus = new SimTK::OrientationSensors(); // add markers addCustomMarkers(model, *markers, *imus); // result storage OpenSim::Kinematics kinematics(&model); kinematics.setRecordAccelerations(true); // move to initial target ik.adoptAssemblyGoal(imus); imus->moveOneObservation(m_humerus_ox, SimTK::Rotation( SimTK::BodyOrSpaceType::BodyRotationSequence, imu_trc.getFrame(0).getMarker(0)[0], SimTK::XAxis, imu_trc.getFrame(0).getMarker(0)[1], SimTK::YAxis, imu_trc.getFrame(0).getMarker(0)[2], SimTK::ZAxis)); imus->moveOneObservation(m_radius_ox, SimTK::Rotation( SimTK::BodyOrSpaceType::BodyRotationSequence, imu_trc.getFrame(0).getMarker(1)[0], SimTK::XAxis, imu_trc.getFrame(0).getMarker(1)[1], SimTK::YAxis, imu_trc.getFrame(0).getMarker(1)[2], SimTK::ZAxis)); // setup inverse kinematics state.setTime(imu_trc.getFrame(0).getFrameTime()); ik.initialize(state); ik.assemble(state); kinematics.begin(state); // loop for every observation frame (!!!must be same length) for (int i = 1; i < imu_trc.getNumFrames(); ++i) { OpenSim::MarkerFrame osensor_frame = imu_trc.getFrame(i); SimTK::Vec3 humerus_vec = osensor_frame.getMarker(0); imus->moveOneObservation(m_humerus_ox, SimTK::Rotation( SimTK::BodyOrSpaceType::BodyRotationSequence, humerus_vec[0], SimTK::XAxis, humerus_vec[1], SimTK::YAxis, humerus_vec[2], SimTK::ZAxis)); SimTK::Vec3 radius_vec = osensor_frame.getMarker(1); imus->moveOneObservation(m_radius_ox, SimTK::Rotation( SimTK::BodyOrSpaceType::BodyRotationSequence, radius_vec[0], SimTK::XAxis, radius_vec[1], SimTK::YAxis, radius_vec[2], SimTK::ZAxis)); // track state.updTime() = osensor_frame.getFrameTime(); ik.track(state.getTime()); ik.updateFromInternalState(state); // report #ifdef DEBUG std::cout << "Frame: " << i << " (t=" << state.getTime() << ")\n"; std::cout << "Error: " << ik.calcCurrentErrorNorm() << "\n"; std::flush(std::cout); #endif // store kinematics.step(state, i); } kinematics.end(state); // store results kinematics.printResults(m_ik_result_path, ""); }
// Returns an instanceHandle of a MemoryPool object. // It creates a MemoryPool instance when the first time // this function is called. instanceOop MemoryPool::get_memory_pool_instance(TRAPS) { // Must do an acquire so as to force ordering of subsequent // loads from anything _memory_pool_obj points to or implies. instanceOop pool_obj = (instanceOop)OrderAccess::load_ptr_acquire(&_memory_pool_obj); if (pool_obj == NULL) { // It's ok for more than one thread to execute the code up to the locked region. // Extra pool instances will just be gc'ed. klassOop k = Management::sun_management_ManagementFactory_klass(CHECK_NULL); instanceKlassHandle ik(THREAD, k); Handle pool_name = java_lang_String::create_from_str(_name, CHECK_NULL); jlong usage_threshold_value = (_usage_threshold->is_high_threshold_supported() ? 0 : -1L); jlong gc_usage_threshold_value = (_gc_usage_threshold->is_high_threshold_supported() ? 0 : -1L); JavaValue result(T_OBJECT); JavaCallArguments args; args.push_oop(pool_name); // Argument 1 args.push_int((int) is_heap()); // Argument 2 symbolHandle method_name = vmSymbolHandles::createMemoryPool_name(); symbolHandle signature = vmSymbolHandles::createMemoryPool_signature(); args.push_long(usage_threshold_value); // Argument 3 args.push_long(gc_usage_threshold_value); // Argument 4 JavaCalls::call_static(&result, ik, method_name, signature, &args, CHECK_NULL); instanceOop p = (instanceOop) result.get_jobject(); instanceHandle pool(THREAD, p); { // Get lock since another thread may have create the instance MutexLocker ml(Management_lock); // Check if another thread has created the pool. We reload // _memory_pool_obj here because some other thread may have // initialized it while we were executing the code before the lock. // // The lock has done an acquire, so the load can't float above it, // but we need to do a load_acquire as above. pool_obj = (instanceOop)OrderAccess::load_ptr_acquire(&_memory_pool_obj); if (pool_obj != NULL) { return pool_obj; } // Get the address of the object we created via call_special. pool_obj = pool(); // Use store barrier to make sure the memory accesses associated // with creating the pool are visible before publishing its address. // The unlock will publish the store to _memory_pool_obj because // it does a release first. OrderAccess::release_store_ptr(&_memory_pool_obj, pool_obj); } } return pool_obj; }