Skip to content

Commit

Permalink
[Gimbalrotor][dragonfly] workaround to enable aerial transformation o…
Browse files Browse the repository at this point in the history
…f dragonfly
  • Loading branch information
sugihara-16 committed May 16, 2024
1 parent 30679dd commit 0515454
Show file tree
Hide file tree
Showing 4 changed files with 5 additions and 5 deletions.
4 changes: 2 additions & 2 deletions robots/gimbalrotor/config/dragonfly/Servo.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ servo_controller:
name: head_joint
angle_sgn: -1
simulation:
pid: {p: 50.0, i: 0.01, d: 1.0}
pid: {p: 20.0, i: 0.1, d: 1.0}
init_value: 0
type: effort_controllers/JointPositionController

Expand All @@ -27,7 +27,7 @@ servo_controller:
name: tail_joint
angle_sgn: -1
simulation:
pid: {p: 50.0, i: 0.01, d: 1.0}
pid: {p: 20.0, i: 0.1, d: 1.0}
init_value: 0
type: effort_controllers/JointPositionController

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@

using namespace aerial_robot_model;

class GimbalrotorRobotModel : public aerial_robot_model::RobotModel{
class GimbalrotorRobotModel : public transformable::RobotModel{
public:
GimbalrotorRobotModel(bool init_with_rosparam = true,
bool verbose = false,
Expand Down
2 changes: 1 addition & 1 deletion robots/gimbalrotor/src/control/gimbalrotor_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -273,7 +273,7 @@ namespace aerial_robot_control
gimbal_state_msg.name.push_back(gimbal_pitch_name);
}
}
gimbal_state_pub_.publish(gimbal_state_msg);
// gimbal_state_pub_.publish(gimbal_state_msg);

}

Expand Down
2 changes: 1 addition & 1 deletion robots/gimbalrotor/src/model/gimbalrotor_robot_model.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ void GimbalrotorRobotModel::updateRobotModelImpl(const KDL::JntArray& joint_posi
KDL::Frame f_baselink;
fk_solver.JntToCart(joint_positions, f_baselink, getBaselinkName());
const KDL::Rotation cog_frame = f_baselink.M * getCogDesireOrientation<KDL::Rotation>().Inverse();
RobotModel::updateRobotModelImpl(joint_positions);
transformable::RobotModel::updateRobotModelImpl(joint_positions);
const auto seg_tf_map = getSegmentsTf();

/* get local coords of thrust links */
Expand Down

0 comments on commit 0515454

Please sign in to comment.