VizMidi::VizMidi() : Vizlet() {
_midiparams = defaultParams();
_midiparams->shape.set("line");
double fadetime = 2.0;
_midiparams->speedinitial.set(0.05);
_midiparams->sizeinitial.set(0.1);
_midiparams->sizefinal.set(0.05);
_midiparams->sizetime.set(fadetime);
_midiparams->lifetime.set(fadetime);
_midiparams->thickness.set(3.0);
// _midiparams->rotauto.set(true);
_midiparams->rotangspeed.set(30.0);
_midiparams->alphainitial.set(0.8);
_midiparams->alphafinal.set(0.0);
_midiparams->alphatime.set(fadetime);
_midiparams->gravity.set(true);
_midiparams->mass.set(0.01);
}
void construct(Params params = defaultParams()){
// get kinematics group name
// get number of joints
VrepRobotArmDriverP_ = std::make_shared<vrep::VrepRobotArmDriver>();
VrepRobotArmDriverP_->construct();
ikGroupHandle_ = simGetIkGroupHandle(std::get<IKGroupName>(params).c_str());
simSetExplicitHandling(ikGroupHandle_,1); // enable explicit handling for the ik
this->currentKinematicsStateP_->Name = std::get<IKGroupName>(params);
this->desiredKinematicsStateP_->Name = std::get<DesiredKinematicsObjectName>(params);
/// @todo set desiredKinematicsStateP name, INITIALIZE ALL MEMBER OBJECTS, & SET NAMES
//
positionLimitsName = std::get<IKGroupName>(params)+"_PositionLimits";
this->jointPositionLimitsVFP_->Name = positionLimitsName;
velocityLimitsName = std::get<IKGroupName>(params)+"_VelocityLimits";
this->jointVelocityLimitsVFP_->Name = velocityLimitsName;
// /// This will hold the Jacobian
// std::unique_ptr<prmKinematicsState> currentKinematicsStateP_;
//
// /// This will hold the xyz position and the rotation of where I want to go
// std::unique_ptr<prmKinematicsState> desiredKinematicsStateP_;
//
// // want to follow a goal position
// /// @todo will want to use an addVFFollow member function once it is added in
// std::unique_ptr<mtsVFDataBase> followVFP_;
//
// /// need velocity limits for the joints
// std::unique_ptr<mtsVFDataJointLimits> jointVelocityLimitsVFP_;
// /// joints cannot go to certain position
// std::unique_ptr<mtsVFDataJointLimits> jointPositionLimitsVFP_;
/// @todo verify object lifetimes
//parent_type::parent_type::InitializeKinematicsState(this->currentKinematicsStateP_)
// for each virtual fixture need names and number of rows
/// @todo read objective rows from vrep
// Initialize Variables for update
//jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
//int numJoints =jointHandles_.size();
#ifndef IGNORE_ROTATION
/// @todo objectiveRows seems to currently be a 3 vector, may eventually want a rotation and translation, perhaps with quaternion rotation
followVFP_->ObjectiveRows = 6;
#else
followVFP_->ObjectiveRows = 3;
#endif
// set the names once for each object, only once
followVFP_->KinNames.push_back(currentKinematicsStateP_->Name);
followVFP_->KinNames.push_back(desiredKinematicsStateP_->Name);
AddVFFollowPath(*followVFP_);
/// @todo set vrep explicit handling of IK here, plus unset in destructor of this object
}
