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