// write the actuator state to the actuator buffer
void write_state( void ) {
//if( VERBOSE ) printf( "(dynamics::write_state)\n" );
// get a reference to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
// write the actuator state out to the command buffer
actuator_msg_c msg = actuator_msg_c( );
msg.header.type = ACTUATOR_MSG_REPLY;
msg.state.position = pivot->q[0];
msg.state.velocity = pivot->qd[0];
msg.state.time = sim->current_time;
// this was a part of the Dynamic ode_both(.) procedure but if the accumulators are
// cleared there they can never change from zero as they were also originally added there.
// So, had to relocate the reset procedure here.
pendulum->reset_accumulators();
// Note: will block on acquiring mutex
if( dyn_amsgbuffer.write( msg ) != BUFFER_ERROR_NONE ) {
sprintf( strbuffer, "(dynamics.cpp) write_state() failed calling actuator_msg_buffer_c.write(msg)\n" );
dyn_error_log.write( strbuffer );
// TODO : return error condition
}
}
/**
maybe a misnomer. Externalized, it means to get the command from the command buffer and
act in response (apply a torque). Not necessary to expose this anymore as an external
interface
*/
void get_command( void ) {
//printf( "(dynamics::get_command) " );
// get the command from the controller
Command cmd = cmdbuffer.read( );
//cmd.print();
// apply any force determined by the controller to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
VectorN tau( 1 );
tau[0] = cmd.state.torque;
pivot->add_force( tau );
}
/**
maybe a misnomer. Externalized, it means to get the command from the command buffer and
act in response (apply a torque). Not necessary to expose this anymore as an external
interface
*/
void get_command( void ) {
if( VERBOSE ) printf( "(dynamics::get_command)\n" );
// get the command from the controller
ActuatorMessage msg = amsgbuffer.read( );
// Note: will block on acquiring mutex
//printf( "(dynamics::get_command) " );
//msg.print();
// apply any force determined by the controller to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
VectorN tau( 1 );
tau[0] = msg.command.torque;
pivot->add_force( tau );
}
/**
publish the state of the actuators to the command buffers. In this case, one actuator
and one controller.
*/
void publish_state( void ) {
//printf( "(dynamics::publish_state)\n" );
// get a reference to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
// write the actuator state out to the command buffer
Command cmd = Command( );
cmd.state.position = pivot->q[0];
cmd.state.velocity = pivot->qd[0];
cmd.state.time = sim->current_time;
//cmd.print();
cmdbuffer.write( cmd );
// this was a part of the Dynamic ode_both(.) procedure but if the accumulators are
// cleared there they can never change from zero. So, had to relocate the reset procedure
// here.
pendulum->reset_accumulators();
}
/// Controls the pendulum
void control_PD( RCArticulatedBodyPtr pendulum, Real time ) {
const Real Kp = 1.0;
const Real Kv = 0.1;
JointPtr pivot = pendulum->find_joint( "pivot" );
if( !pivot )
std::cerr << "Could not find pivot joint\n";
Real measured_position = pivot->q[0];
Real measured_velocity = pivot->qd[0];
Real desired_position = position_trajectory( time, 0.5, 0.0, PI );
Real desired_velocity = velocity_trajectory( time, 0.5, 0.0, PI );
Real torque = Kp * ( desired_position - measured_position ) + Kv * ( desired_velocity - measured_velocity );
VectorN tau( 1 );
tau[0] = torque;
pivot->add_force( tau );
}
// Read the command ( torque ) from the message buffer.
void read_command( void ) {
actuator_msg_c msg;
//if( VERBOSE ) printf( "(dynamics::read_command)\n" );
// get the command from the controller
// Note: will block on acquiring mutex
if( dyn_amsgbuffer.read( msg ) != BUFFER_ERROR_NONE ) {
sprintf( strbuffer, "(dynamics.cpp) read_command() failed calling actuator_msg_buffer_c.read(msg)\n" );
dyn_error_log.write( strbuffer );
// TODO : return error condition
}
//If( VERBOSE ) msg.print();
// apply any force determined by the controller to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
Ravelin::VectorNd tau( 1 );
tau[0] = msg.command.torque;
pivot->add_force( tau );
}
/**
publish the state of the actuators to the command buffers. In this case, one actuator
and one controller.
*/
void publish_state( void ) {
if( VERBOSE ) printf( "(dynamics::publish_state)\n" );
// get a reference to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
// write the actuator state out to the command buffer
ActuatorMessage msg = ActuatorMessage( );
msg.header.type = MSG_TYPE_STATE_REPLY;
msg.state.position = pivot->q[0];
msg.state.velocity = pivot->qd[0];
msg.state.time = sim->current_time;
// this was a part of the Dynamic ode_both(.) procedure but if the accumulators are
// cleared there they can never change from zero as they were also originally added there.
// So, had to relocate the reset procedure here.
pendulum->reset_accumulators();
// Note: will block on acquiring mutex
amsgbuffer.write( msg );
}