-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
lcm_plan_interpolator.h
68 lines (57 loc) · 1.91 KB
/
lcm_plan_interpolator.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
#pragma once
#include <string>
#include "drake/manipulation/util/robot_plan_interpolator.h"
#include "drake/systems/framework/diagram.h"
namespace drake {
namespace examples {
namespace kuka_iiwa_arm {
/**
* A Diagram that adapts a RobotPlanInterpolator for use with an Iiwa arm.
*
* @system
* name: LcmPlanInterpolator
* input_ports:
* - status_receiver_lcmt_iiwa_status
* - plan_interpolator_plan
* output_ports:
* - command_sender_lcmt_iiwa_command
* @endsystem
*
* @see `lcmt_iiwa_status.lcm`, `lcmt_iiwa_command.lcm`, `lcmt_robot_plan.lcm`,
* and `lcmt_robot_state.lcm` for additional documentation.
*/
class LcmPlanInterpolator : public systems::Diagram<double> {
public:
LcmPlanInterpolator(
const std::string& model_path,
manipulation::util::InterpolatorType interpolator_type);
const systems::InputPort<double>& get_input_port_iiwa_status()
const {
return get_input_port(input_port_iiwa_status_);
}
const systems::InputPort<double>& get_input_port_iiwa_plan() const {
return get_input_port(input_port_iiwa_plan_);
}
const systems::OutputPort<double>& get_output_port_iiwa_command() const {
return get_output_port(output_port_iiwa_command_);
}
/**
* Makes a plan to hold at the measured joint configuration @p q0 starting at
* @p plan_start_time. This function needs to be explicitly called before any
* simulation. Otherwise this aborts in CalcOutput().
*/
void Initialize(double plan_start_time, const VectorX<double>& q0,
systems::Context<double>* context) const;
int num_joints() const { return num_joints_; }
private:
// Input ports.
int input_port_iiwa_status_{-1};
int input_port_iiwa_plan_{-1};
// Output ports.
int output_port_iiwa_command_{-1};
manipulation::util::RobotPlanInterpolator* robot_plan_interpolator_{};
int num_joints_{};
};
} // namespace kuka_iiwa_arm
} // namespace examples
} // namespace drake