/// Provides new values that the body should use to control it's actuators. void Else::AcrobotArticulatedBody ::setActuators( const ControllerPtr argController ) { // 1 actuator double torque = 0.85 * scaleTorque( argController->getOutput(1, 0) ); dJointAddHingeTorque( myJoints[0], torque ); }
void soy_joints_hinge_addTorque (soyjointsHinge* self, dReal torque) { struct dxJoint* _tmp0_; dReal _tmp1_; g_return_if_fail (self != NULL); _tmp0_ = ((soyjointsJoint*) self)->joint; _tmp1_ = torque; dJointAddHingeTorque ((struct dxJoint*) _tmp0_, _tmp1_); }
/** *@brief ジョイントの力制御の関数 * @param v トルク、力 */ void ODEJointObj::ControlJointToq(double v) { ms->mu->lock(); if(JointType == 0) { dJointAddSliderForce(joint, v); } else if(JointType == 2) { dJointAddHingeTorque(joint, v); } ms->mu->unlock(); }
void soy_joints_hinge_addTorque (soyjointsHinge* self, dReal torque) { struct dxJoint* _tmp0_ = NULL; dReal _tmp1_ = 0.0; #line 43 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs" g_return_if_fail (self != NULL); #line 44 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs" _tmp0_ = ((soyjointsJoint*) self)->joint; #line 44 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs" _tmp1_ = torque; #line 44 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs" dJointAddHingeTorque ((struct dxJoint*) _tmp0_, _tmp1_); #line 334 "Hinge.c" }
void ODE_LinearSpringModel::updateInputValues() { if(mOwnerSpringAdapter == 0 || mOwnerSpringAdapter->getCurrentTargetJoint() == 0) { return; } //The first time an update is run the mJoint is located. //This has to be done here instead of setup() to ensure that the joint is //really available, which can not be guaranteed in setup(). if(mJoint == 0) { SimJoint *simJoint = mOwnerSpringAdapter->getCurrentTargetJoint(); ODE_Joint *odeJoint = dynamic_cast<ODE_Joint*>(simJoint); if(odeJoint == 0) { MotorAdapter *adapter = dynamic_cast<MotorAdapter*>(simJoint); if(adapter != 0) { odeJoint = dynamic_cast<ODE_Joint*>(adapter->getActiveMotorModel()); } } if(simJoint != 0 && odeJoint != 0) { if(getType() == MotorModel::HINGE_JOINT) { if(dynamic_cast<HingeJoint*>(simJoint) != 0) { mJoint = odeJoint->getJoint(); } } else if(getType() == MotorModel::SLIDER_JOINT) { if(dynamic_cast<SliderJoint*>(simJoint) != 0) { mJoint = odeJoint->getJoint(); } } } } if(mJoint != 0) { if(getType() == MotorModel::HINGE_JOINT) { double torque = calculateTorque(dJointGetHingeAngle(mJoint)); dJointAddHingeTorque(mJoint, torque); mOwnerSpringAdapter->getCurrentTorqueValue()->set(torque); } else if(getType() == MotorModel::SLIDER_JOINT) { double force = calculateForce(dJointGetSliderPosition(mJoint)); dJointAddSliderForce(mJoint, force); mOwnerSpringAdapter->getCurrentTorqueValue()->set(force); } } }
//! This function is called once per simulation step, allowing the //! constraint to be "motorized" according to some response. It runs //! in the physics thread. void OscHingeODE::simulationCallback() { ODEConstraint& me = *static_cast<ODEConstraint*>(special()); dReal angle = dJointGetHingeAngle(me.joint()); dReal rate = dJointGetHingeAngleRate(me.joint()); dReal addtorque = - m_response->m_stiffness.m_value*angle - m_response->m_damping.m_value*rate; // Limit the torque otherwise we get ODE assertions. if (addtorque > 1000) addtorque = 1000; if (addtorque < -1000) addtorque = -1000; m_torque.m_value = addtorque; dJointAddHingeTorque(me.joint(), addtorque); }
//! Add torque to joint void HingeJoint::addTorque(double torque) { // - is required, because the direction is opposite from setAngle dJointAddHingeTorque(m_joint,-torque); }
int ToyControl::action() { if (!_sim) { return -1; } /* _target[0] = -1; _target[1] = s0.swh; _target[2] = -s0.swk; _target[3] = -s0.stk; _target[4] = s0.swa; _target[5] = s0.sta; */ // collision detection dContactGeom contact[100]; dGeomID leftfootId = _toy.box[5].id(); dGeomID rightfootId = _toy.box[6].id(); dGeomID groundId = _toy._env.ground.id(); int left = dCollide(leftfootId, groundId, 100, &contact[0], sizeof(dContactGeom)); int right = dCollide(rightfootId, groundId, 100, &contact[1], sizeof(dContactGeom)); // end of collision detection //target->joint //0 left hip //1 right hip //2 left knee //3 right knee //4 left ankle //5 right ankle if (now.num==0){ if (diff >= now.deltaT){ now = s1; before = clock(); std::cout<<"0->1"<<std::endl; }else if (right > 0){ now = s2; before = clock(); diff = 0; std::cout<<"0->2"<<"\tright "<<right<<std::endl; }else{ //left lift _target[0] = -s0.swh; _target[1] = s0.swh; _target[2] = -s0.swk;//- _target[3] = s0.stk; _target[4] = s0.swa; _target[5] = s0.sta; } } if(now.num==1){ if (left>0){ std::cout<<"1->2"<<"\tleft "<<left<<std::endl; now = s2; before = clock(); diff = 0; }else{ //left contact _target[0] = -s1.swh; _target[1] = s1.swh; _target[2] = -s1.swk;//- _target[3] = s1.stk; _target[4] = s1.swa; _target[5] = s1.sta; } } if(now.num==2){ if (diff >= now.deltaT){ now = s3; before = clock(); std::cout<<"2->3"<<std::endl; }else if (left > 0){ now = s0; before = clock(); diff = 0; std::cout<<"2->0"<<"\tleft "<<left<<std::endl; }else{ //right lift _target[1] = -s2.swh; _target[0] = s2.swh; _target[3] = -s2.swk;//- _target[2] = s2.stk; _target[5] = s2.swa; _target[4] = s2.sta; } } if(now.num==3){ if (right>0){ std::cout<<"3->0"<<"\tright "<<right<<std::endl; now = s0; before = clock(); diff = 0; }else{ _target[1] = -s3.swh; _target[0] = s3.swh; _target[3] = -s3.swk;//- _target[2] = s3.stk; _target[5] = s3.swa; _target[4] = s3.sta; } } if(now.num==4){ std::cout<<"starting..."<<std::endl; before = clock(); _target[0] = -s0.swh; _target[1] = s0.swh; _target[2] = -s0.swk;//- _target[3] = s0.stk; _target[4] = s0.swa; _target[5] = s0.sta; now.num=0; } // Add joint torques to each DOF, pulling the body towards the // desired state defined by _target. for (int i = 0; i < NUM_BODY-1; i++) { dJointID jt = _toy.joint[i].id(); double limit = _ks[i]; dReal theta = dJointGetHingeAngle(jt); dReal thetav = dJointGetHingeAngleRate(jt); dReal torque = _ks[i]*(_target[i] - theta) - _kd[i]*thetav; // PD controler equation if (torque > limit) torque = limit; if (torque < -limit) torque = -limit; // a limit on torque dJointAddHingeTorque(jt, torque); //use this torque in the next step of simulation } after = clock(); diff = ((float)(after-before))/CLOCKS_PER_SEC; return 0; }
void CODEHingeJoint::addTorque(double torque, int axisIndex) { dJointAddHingeTorque(mID, torque); }
void ODE_Link::setTorque(dReal t){ if(jointType == ODE_Link::ROTATIONAL_JOINT) return dJointAddHingeTorque(odeJointId, t); else if(jointType == ODE_Link::SLIDE_JOINT) return dJointAddSliderForce(odeJointId, t); }