36 std::vector<std::string>
physical_joint_names{
"phys_joint_0",
"phys_joint_1",
"phys_joint_2",
"phys_joint_3"};
38 std::vector<std::pair<double, double>>
physical_joint_limits{{-30.0, 30.0}, {-10.0, 10.0}, {-32.0, 33.0}, {0.0, 120.0}};
54 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
56 actAxesPos[0] = physJointsPos[0];
57 actAxesPos[1] = physJointsPos[1];
58 actAxesPos[2] = physJointsPos[2] + physJointsPos[3];
68 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
70 actAxesVel[0] = physJointsVel[0];
71 actAxesVel[1] = physJointsVel[1];
72 actAxesVel[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3];
83 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
85 actAxesAcc[0] = physJointsAcc[0];
86 actAxesAcc[1] = physJointsAcc[1];
87 actAxesAcc[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3] + physJointsAcc[2] + physJointsAcc[3];
100 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
102 physJointsPos[0] = actAxesPos[0];
103 physJointsPos[1] = actAxesPos[1];
104 physJointsPos[2] = actAxesPos[2] / 2.0;
105 physJointsPos[3] = actAxesPos[2] / 2.0;
114 yCError(
FAKEJOINTCOUPLING) <<
"convertFromActuatedAxesToPhysicalJointsVel: input or output vectors have wrong size";
115 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
117 physJointsVel[0] = actAxesVel[0];
118 physJointsVel[1] = actAxesVel[1];
119 physJointsVel[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0;
120 physJointsVel[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0;
130 yCError(
FAKEJOINTCOUPLING) <<
"convertFromActuatedAxesToPhysicalJointsAcc: input or output vectors have wrong size";
131 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
133 physJointsAcc[0] = actAxesAcc[0];
134 physJointsAcc[1] = actAxesAcc[1];
135 physJointsAcc[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0 - actAxesAcc[2] / 2.0;
136 physJointsAcc[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0 + actAxesAcc[2] / 2.0;
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
yarp::dev::ReturnValue convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector &physJointsPos, const yarp::sig::Vector &physJointsVel, const yarp::sig::Vector &physJointsAcc, yarp::sig::Vector &actAxesAcc) override
yarp::dev::ReturnValue convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector &actAxesPos, const yarp::sig::Vector &actAxesVel, yarp::sig::Vector &physJointsVel) override
bool open(yarp::os::Searchable &par) override
Open the DeviceDriver.
yarp::dev::ReturnValue convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector &physJointsPos, yarp::sig::Vector &actAxesPos) override
yarp::dev::ReturnValue convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector &actAxesPos, yarp::sig::Vector &physJointsPos) override
yarp::dev::ReturnValue convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector &physJointsPos, const yarp::sig::Vector &physJointsVel, yarp::sig::Vector &actAxesVel) override
yarp::dev::ReturnValue convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector &actAxesPos, const yarp::sig::Vector &actAxesTrq, yarp::sig::Vector &physJointsTrq) override
yarp::dev::ReturnValue convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector &actAxesPos, const yarp::sig::Vector &actAxesVel, const yarp::sig::Vector &actAxesAcc, yarp::sig::Vector &physJointsAcc) override
yarp::dev::ReturnValue convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector &physJointsPos, const yarp::sig::Vector &physJointsTrq, yarp::sig::Vector &actAxesTrq) override
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue getNrOfActuatedAxes(size_t &nrOfActuatedAxes) override final
yarp::dev::ReturnValue getNrOfPhysicalJoints(size_t &nrOfPhysicalJoints) override final
void initialise(yarp::sig::VectorOf< size_t > coupled_physical_joints, yarp::sig::VectorOf< size_t > coupled_actuated_axes, std::vector< std::string > physical_joint_names, std::vector< std::string > actuated_axes_names, std::vector< std::pair< double, double > > coupled_physical_joint_limits)
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
size_t size() const
Returns the number of elements in the vector.
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
The components from which ports and connections are built.
An interface to the operating system, including Port based communication.
typename vector_selector< T >::type VectorOf