void apply_any_constraints(RecordedExpectation *expectation, const char *parameter, intptr_t actual) {
int i;
for (i = 0; i < cgreen_vector_size(expectation->constraints); i++) {
Constraint *constraint = (Constraint *)cgreen_vector_get(expectation->constraints, i);
if (is_constraint_parameter(constraint, parameter)) {
switch(constraint->constraint_type)
{
case CG_CONSTRAINT_WANT:
test_constraint(
constraint,
expectation->function,
actual,
expectation->test_file,
expectation->test_line,
get_test_reporter());
break;
case CG_CONSTRAINT_SET:
*((int *)actual) = (int)constraint->out_value;
break;
case CG_CONSTRAINT_FILL:
memcpy((void *)actual, (void *)constraint->out_value, constraint->copy_size);
break;
}
}
}
}
int main(void) {
init_rationals();
init_vartable();
show_all_vars(stdout);
test_constraint();
delete_vartable();
cleanup_rationals();
return 0;
}
void apply_any_constraints(RecordedExpectation *expectation, const char *parameter, intptr_t actual) {
int i;
for (i = 0; i < vector_size(expectation->constraints); i++) {
Constraint *constraint = (Constraint *)vector_get(expectation->constraints, i);
if (is_constraint_parameter(constraint, parameter)) {
test_constraint(
constraint,
expectation->function,
actual,
expectation->test_file,
expectation->test_line,
get_test_reporter());
}
}
}
bool CStateInfo::test_constraint( const CValue &value ) const
{
CValue::type endpoint_type = get_type();
if( value.get_type() != endpoint_type )
{
CValue *fixed_type = value.transmogrify( endpoint_type );
bool test_result = test_constraint( *fixed_type );
delete fixed_type;
return test_result;
}
const IConstraint &constraint = get_constraint();
const IValueRange *range = constraint.get_value_range();
if( range )
{
switch( value.get_type() )
{
case CValue::STRING:
{
/* for strings, range constraint defines min/max length */
const string &string_value = value;
int string_length = string_value.length();
if( string_length < ( int ) range->get_minimum() ) return false;
if( string_length > ( int ) range->get_maximum() ) return false;
return true;
}
case CValue::INTEGER:
{
/* for integers, range constraint defines min/max value */
int int_value = value;
if( int_value < ( int ) range->get_minimum() ) return false;
if( int_value > ( int ) range->get_maximum() ) return false;
return true;
}
case CValue::FLOAT:
{
/* for floats, range constraint defines min/max value */
float float_value = value;
if( float_value < ( float ) range->get_minimum() ) return false;
if( float_value > ( float ) range->get_maximum() ) return false;
return true;
}
default:
INTEGRA_TRACE_ERROR << "unhandled value type";
return false;
}
}
else /* allowed value constraint */
{
const value_set *allowed_states = constraint.get_allowed_states();
assert( allowed_states );
for( value_set::const_iterator i = allowed_states->begin(); i != allowed_states->end(); i++ )
{
if( value.is_equal( **i ) )
{
return true;
}
}
return false; //not found
}
}
/*****************************************************************************
******************************************************************************
Function Name : run_test_sine_task
Date : Dec. 1997
Remarks:
run the task from the task servo: REAL TIME requirements!
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
run_test_sine_task(void)
{
int j, i;
double gain = 100;
task_time += 1./(double)task_servo_rate;
for (i=1; i<=n_dofs; ++i) {
joint_des_state[i].th = off[i];
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
for (j=1; j<=n_sine[i]; ++j) {
joint_des_state[i].th +=
amp[i][j] * sin(2.*PI*speed*freq[i][j]*task_time+phase[i][j]);
joint_des_state[i].thd += amp[i][j] * 2.*PI*speed*freq[i][j] *
cos(2.*PI*speed*freq[i][j]*task_time+phase[i][j]);
joint_des_state[i].thdd += -amp[i][j] * sqr(2.*PI*speed*freq[i][j]) *
sin(2.*PI*speed*freq[i][j]*task_time+phase[i][j]);
}
// store these in another variable for mrdplot
target[i].th = joint_des_state[i].th;
target[i].thd = joint_des_state[i].thd;
target[i].thdd = joint_des_state[i].thdd;
if (0) {
// hack
joint_des_state[i].thdd += gain*(joint_des_state[i].th-joint_state[i].th) +
2.*sqrt(gain)*(joint_des_state[i].thd-joint_state[i].thd);
}
}
if (!invdyn_servo_flag) {
// here this function overwrites the sensor readings
test_constraint();
bzero((void *)uext,sizeof(SL_uext)*(n_dofs+1));
mult = 0.9;
uext[L_AAA].f[_X_] = misc_sensor[L_CFx] * mult;
uext[L_AAA].f[_Y_] = misc_sensor[L_CFy] * mult;
uext[L_AAA].f[_Z_] = misc_sensor[L_CFz] * mult;
uext[L_AAA].t[_A_] = misc_sensor[L_CTa] * mult;
uext[L_AAA].t[_B_] = misc_sensor[L_CTb] * mult;
uext[L_AAA].t[_G_] = misc_sensor[L_CTg] * mult;
uext[R_AAA].f[_X_] = misc_sensor[R_CFx] * mult;
uext[R_AAA].f[_Y_] = misc_sensor[R_CFy] * mult;
uext[R_AAA].f[_Z_] = misc_sensor[R_CFz] * mult;
uext[R_AAA].t[_A_] = misc_sensor[R_CTa] * mult;
uext[R_AAA].t[_B_] = misc_sensor[R_CTb] * mult;
uext[R_AAA].t[_G_] = misc_sensor[R_CTg] * mult;
if (which_invdyn == 1)
SL_InverseDynamics(joint_state,joint_des_state,endeff);
else
SL_InverseDynamicsArt(joint_state,joint_des_state,
&base_state,&base_orient,
uext, endeff);
}
//if (task_servo_calls%5000 == 0)
//test_constraint();
if (0) {
for (i=1; i<=N_DOFS; ++i) {
joint_des_state[i].th = joint_state[i].th;
joint_des_state[i].thd = joint_state[i].thd;
}
}
return TRUE;
}
