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;
}
