Expression<Wrench>::Ptr RefPoint_WrenchVector::derivativeExpression(int i) {
Expression<Wrench>::Ptr a = cached<Wrench>(argument1);
Expression<Wrench>::Ptr da = cached<Wrench>(argument1->derivativeExpression(i));
Expression<Vector>::Ptr b = cached<Vector>(argument2);
Expression<Vector>::Ptr db = cached<Vector>(argument2->derivativeExpression(i));
return wrench( force(da),
torque(da) + force(da)*b + force(a)*db );
}
Expression<Wrench>::Ptr Composition_RotationWrench::derivativeExpression(int i) {
Expression<Rotation>::Ptr a = cached<Rotation>(argument1);
Expression<Vector>::Ptr da = cached<Vector>(argument1->derivativeExpression(i));
Expression<Wrench>::Ptr b = cached<Wrench>(argument2);
Expression<Wrench>::Ptr db = cached<Wrench>(argument2->derivativeExpression(i));
return wrench( a*force(db) + da*(a*force(b)),
a*torque(db) + da*(a*torque(b)) );
}
std::vector<double> QualPCR::getGravityWrench(Grasp *grasp)
{
std::vector<double> wrench(6, 0.0);
vec3 hand_z(0,0,1);// * grasp->getHand()->getTran();
wrench[0] = hand_z.x();
wrench[1] = hand_z.y();
wrench[2] = hand_z.z();
return wrench;
}
sensor_msgs::JointState ThrusterAllocator::wrench2Thrusters(const geometry_msgs::Wrench &cmd)
{
thrust_msg.header.stamp = ros::Time::now();
Eigen::VectorXd wrench(6);
wrench << cmd.force.x, cmd.force.y, cmd.force.z,
cmd.torque.x, cmd.torque.y, cmd.torque.z;
Eigen::VectorXd thrust = inverse_map * wrench;
saturate(thrust);
for(int i = 0; i < thrust.rows(); i++)
thrust_msg.effort[i] = thrust[i];
return thrust_msg;
}
Expression<Wrench>::Ptr Wrench_VectorVector::derivativeExpression(int i)
{
return wrench( argument1, argument2 );
}
/*!
Builds a PCR qm parameter area within the quality measure dialog
box such that the user can input the applied wrench and the
desired maximum contact force
*/
void
QualPCR::buildParamArea(qmDlgDataT *qmData)
{
static QualPCRParamT params;
QualPCR *currQM = (QualPCR*)qmData->currQM;
#ifdef GRASPITDBG
std::cout << "building QualPCR" << std::endl;
#endif
std::vector<double> wrench(6, 0.0);
double wrenchMultiplier = 1.0;
double maxForce = 10.0;
if (currQM) {
wrench = currQM->mWrench;
wrenchMultiplier = currQM->mWrenchMultiplier;
maxForce = currQM->mMaxForce;
} else if (qmData->grasp->isGravitySet()) {
wrench = getGravityWrench(qmData->grasp);
wrenchMultiplier = qmData->grasp->getObject()->getMass() * 1.0e-3 * 9.80665 * 1.0e6;
}
QGridLayout *wl = new QGridLayout(qmData->settingsArea,8,2);
wl->addWidget(new QLabel(QString("Multiplier:")));
params.wrenchInput = new QLineEdit();
params.wrenchInput->setText(QString::number(wrenchMultiplier));
wl->addWidget(params.wrenchInput);
wl->addWidget(new QLabel(QString("Force X:")));
params.FxInput = new QLineEdit();
params.FxInput->setText(QString::number(wrench[0]));
wl->addWidget(params.FxInput);
wl->addWidget(new QLabel(QString("Force Y:")));
params.FyInput = new QLineEdit();
params.FyInput->setText(QString::number(wrench[1]));
wl->addWidget(params.FyInput);
wl->addWidget(new QLabel(QString("Force Z:")));
params.FzInput = new QLineEdit();
params.FzInput->setText(QString::number(wrench[2]));
wl->addWidget(params.FzInput);
wl->addWidget(new QLabel(QString("Torque X:")));
params.MxInput = new QLineEdit();
params.MxInput->setText(QString::number(wrench[3]));
wl->addWidget(params.MxInput);
wl->addWidget(new QLabel(QString("Torque Y:")));
params.MyInput = new QLineEdit();
params.MyInput->setText(QString::number(wrench[4]));
wl->addWidget(params.MyInput);
wl->addWidget(new QLabel(QString("Torque Z:")));
params.MzInput = new QLineEdit();
params.MzInput->setText(QString::number(wrench[5]));
wl->addWidget(params.MzInput);
wl->addWidget(new QLabel(QString("Maximum Force:")));
params.maxForceInput = new QLineEdit();
params.maxForceInput->setText(QString::number(maxForce));
wl->addWidget(params.maxForceInput);
qmData->paramPtr = ¶ms;
}
/*!
* \brief Unpack force/torque ADC samples from realtime data.
*
* \return True, if there are no problems, false if there is something wrong with the data.
*/
bool WG06::unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *last_status)
{
pr2_hardware_interface::ForceTorqueState &ft_state(force_torque_.state_);
ros::Time current_time(ros::Time::now());
// Fill in raw analog output with most recent data sample, (might become deprecated?)
{
ft_raw_analog_in_.state_.state_.resize(6);
const FTDataSample &sample(status->ft_samples_[0]);
for (unsigned i=0; i<6; ++i)
{
int raw_data = sample.data_[i];
ft_raw_analog_in_.state_.state_[i] = double(raw_data);
}
}
unsigned new_samples = (unsigned(status->ft_sample_count_) - unsigned(last_status->ft_sample_count_)) & 0xFF;
ft_sample_count_ += new_samples;
int missed_samples = std::max(int(0), int(new_samples) - 4);
ft_missed_samples_ += missed_samples;
unsigned usable_samples = min(new_samples, MAX_FT_SAMPLES);
// Also, if number of new_samples is ever 0, then there is also an error
if (usable_samples == 0)
{
ft_sampling_rate_error_ = true;
}
// Make room in data structure for more f/t samples
ft_state.samples_.resize(usable_samples);
// If any f/t channel is overload or the sampling rate is bad, there is an error.
ft_state.good_ = ( (!ft_sampling_rate_error_) &&
(ft_overload_flags_ == 0) &&
(!ft_disconnected_) &&
(!ft_vhalf_error_) );
for (unsigned sample_index=0; sample_index<usable_samples; ++sample_index)
{
// samples are stored in status data, so that newest sample is at index 0.
// this is the reverse of the order data is stored in hardware_interface::ForceTorque buffer.
unsigned status_sample_index = usable_samples-sample_index-1;
const FTDataSample &sample(status->ft_samples_[status_sample_index]);
geometry_msgs::Wrench &wrench(ft_state.samples_[sample_index]);
convertFTDataSampleToWrench(sample, wrench);
}
// Put newest sample into analog vector for controllers (deprecated)
if (usable_samples > 0)
{
const geometry_msgs::Wrench &wrench(ft_state.samples_[usable_samples-1]);
ft_analog_in_.state_.state_[0] = wrench.force.x;
ft_analog_in_.state_.state_[1] = wrench.force.y;
ft_analog_in_.state_.state_[2] = wrench.force.z;
ft_analog_in_.state_.state_[3] = wrench.torque.x;
ft_analog_in_.state_.state_[4] = wrench.torque.y;
ft_analog_in_.state_.state_[5] = wrench.torque.z;
}
// Put all new samples in buffer and publish it
if ((raw_ft_publisher_ != NULL) && (raw_ft_publisher_->trylock()))
{
raw_ft_publisher_->msg_.samples.resize(usable_samples);
raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
raw_ft_publisher_->msg_.missed_samples = ft_missed_samples_;
for (unsigned sample_num=0; sample_num<usable_samples; ++sample_num)
{
// put data into message so oldest data is first element
const FTDataSample &sample(status->ft_samples_[sample_num]);
ethercat_hardware::RawFTDataSample &msg_sample(raw_ft_publisher_->msg_.samples[usable_samples-sample_num-1]);
msg_sample.sample_count = ft_sample_count_ - sample_num;
msg_sample.data.resize(NUM_FT_CHANNELS);
for (unsigned ch_num=0; ch_num<NUM_FT_CHANNELS; ++ch_num)
{
msg_sample.data[ch_num] = sample.data_[ch_num];
}
msg_sample.vhalf = sample.vhalf_;
}
raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
raw_ft_publisher_->unlockAndPublish();
}
// Put newest sample in realtime publisher
if ( (usable_samples > 0) && (ft_publisher_ != NULL) && (ft_publisher_->trylock()) )
{
ft_publisher_->msg_.header.stamp = current_time;
ft_publisher_->msg_.wrench = ft_state.samples_[usable_samples-1];
ft_publisher_->unlockAndPublish();
}
// If this returns false, it will cause motors to halt.
// Return "good_" state of sensor, unless halt_on_error is false.
return ft_state.good_ || !force_torque_.command_.halt_on_error_;
}
