44 JointGroupVelocityController::JointGroupVelocityController()
45 : robot_(NULL), loop_count_(0)
73 for (
int i = 0;
i < joint_names.
size(); ++
i)
76 if (name_value.
getType() != XmlRpcValue::TypeString)
78 ROS_ERROR(
"Array of joint names should contain all strings. (namespace: %s)",
85 ROS_ERROR(
"Joint not found: %s. (namespace: %s)",
110 (
node_,
"statearray", 1));
127 std::vector< std::string > joint_names;
130 joint_names.push_back(
joints_[
i]->joint_->name);
159 std::vector<double> compute_command(
n_joints_);
160 std::vector<double> compute_error(
n_joints_);
164 compute_error[
i] = commands[
i] -
joints_[
i]->velocity_;
176 pr2_controllers_msgs::JointControllerState singlejointcontrollerstate;
177 singlejointcontrollerstate.header.stamp = time;
178 singlejointcontrollerstate.set_point = commands[
i];
179 singlejointcontrollerstate.process_value =
joints_[
i]->velocity_;
180 singlejointcontrollerstate.error = compute_error[
i];
181 singlejointcontrollerstate.time_step = dt_.
toSec();
182 singlejointcontrollerstate.command = compute_command[
i];
186 singlejointcontrollerstate.p,
187 singlejointcontrollerstate.i,
188 singlejointcontrollerstate.d,
189 singlejointcontrollerstate.i_clamp,
207 ROS_ERROR_STREAM(
"Dimension of command (" <<
msg->data.size() <<
") does not match number of joints (" <<
n_joints_ <<
")! Not executing!");