Ejemplo n.º 1
0
// 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
        }
    }
Ejemplo n.º 2
0
/**
  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 );
}
Ejemplo n.º 3
0
/**
  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 );
}
Ejemplo n.º 4
0
/**
  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();
}
Ejemplo n.º 5
0
/// 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 );
}
Ejemplo n.º 6
0
//  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 );
    }
Ejemplo n.º 7
0
/**
  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 );
}