void RobotController::SetPIDCommand(const Config& qdes)
{
Config dqdes(qdes.size(),0.0);
SetPIDCommand(qdes,dqdes);
}
void ContactJointTrackingController::Update(Real dt)
{
RobotController::Update(dt);
base->command = command;
base->sensors = sensors;
Config qdes(robot.links.size()),dqdes(robot.links.size());
base->GetDesiredState(qdes,dqdes);
base->Update(dt);
/*
COM tasks
opSpaceController.stateEstimator->UpdateModel();
Vector3 comcur=robot.GetCOM();
robot.UpdateConfig(qdes);
Vector3 comdes=robot.GetCOM();
opSpaceController.comTasks[0].ddxdes(0) = 1.0*(comdes.x-comcur.x);
*/
/*
//Workspace tasks
opSpaceController.stateEstimator->UpdateModel();
Vector3 xcur=robot.links[1].T_World*Vector3(1,0,0);
robot.UpdateConfig(qdes);
Vector3 xdes=robot.links[1].T_World*Vector3(1,0,0);
opSpaceController.workspaceTasks[0].ddxdes.copy((Real*)(1.0*(xdes-xcur)));
*/
//change the (q,dq) command to a ddq command
DesiredToAccel(dt,qdes,dqdes,opSpaceController.jointTasks[0].ddqdes);
opSpaceController.stateEstimator->UpdateModel();
cout<<"Current q: "<<robot.q<<endl;
cout<<"Current dq: "<<robot.dq<<endl;
cout<<"Desired q: "<<qdes<<endl;
cout<<"Desired q': "<<dqdes<<endl;
cout<<"Desired q'': "<<opSpaceController.jointTasks[0].ddqdes<<endl;
/*
//HACK: set estimated translational velocity to zero
robot.dq(0) = robot.dq(1) = robot.dq(2) = 0;
//HACK: set estimated rotational velocity to zero
robot.dq(3) = robot.dq(4) = robot.dq(5) = 0;
*/
opSpaceController.sensors = sensors;
opSpaceController.command = command;
assert(opSpaceController.IsValid());
opSpaceController.Update(dt);
#if !TORQUE_CONTROL_ONLY
//change torque commands to feedforward torques
for(size_t i=0;i<robot.drivers.size();i++) {
Real feedforwardTorque = command->actuators[i].torque;
if(robot.drivers[i].type == RobotJointDriver::Normal) {
command->actuators[i].SetPID(qdes(robot.drivers[i].linkIndices[0]),dqdes(robot.drivers[i].linkIndices[0]),command->actuators[i].iterm);
}
else {
robot.q = qdes;
robot.dq = dqdes;
//printf("Desired affine driver value %g, vel %g\n",robot.GetDriverValue(i),robot.GetDriverVelocity(i));
command->actuators[i].SetPID(robot.GetDriverValue(i),robot.GetDriverVelocity(i),command->actuators[i].iterm);
}
command->actuators[i].torque = feedforwardTorque;
}
#endif
}
