Skip to content

Backend support for real Kinova control with ft and gripper #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
KishanSawant wants to merge 8 commits into
base: dev
Choose a base branch
from
Open

Backend support for real Kinova control with ft and gripper #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
fc7187c
support real Kinova control with ft and gripper
KishanSawant Jul 23, 2026
63dd268
Preserve ROS publishing in generated controllers
KishanSawant Jul 23, 2026
5b84d0a
Use automatic wrench outputs in backend generation
KishanSawant Jul 23, 2026
11f1c4e
Guard peripheral generation on nonempty wrench data
KishanSawant Jul 23, 2026
0bfb8e9
Avoid optional peripheral code for empty wrench lists
KishanSawant Jul 23, 2026
608f82e
Simplify wrench output template guards
KishanSawant Jul 23, 2026
f0b8f41
Use model timestep for Kinova cycle time
KishanSawant Jul 24, 2026
29f2b5c
Finalize Kinova robif2b backend support
KishanSawant Jul 24, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 29 additions & 1 deletion code-generator/introspection.stg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -353,6 +353,10 @@ public:
: shm_(shm_name()),
logger_(log_path(), kRuntimeProducerAgentId, kRuntimeActivityId) {}

// Flush the protobuf writer before a backend tears down its GUI or hardware
// resources. The destructor remains a second, harmless close for early exits.
void close() { logger_.close(); }

void begin_tick(double t, std::uint64_t step, std::int64_t active_motion) {
begin_tick(t, step, active_motion, active_motion);
\}
Expand Down Expand Up @@ -485,8 +489,32 @@ introspection-controller-sample(slot, views) ::= <<
pub.constraint_sample(<slot.uri>, <slot.error_expr>, <slot.output_expr>, <ctrl-sig(slot.measured_signal, views)>, <ctrl-sig(slot.setpoint_signal, views)>, motion_spec::runtime::constraint_satisfied(<slot.error_expr>));
>>

introspection-cond-term(t, motion_id) ::= <<
<({introspection-cond-term-<t.kind>})(t, motion_id)>
>>

introspection-cond-term-elapsed(t, motion_id) ::= <<
(shared.<motion_id>_motion_start_time <t.op> <t.threshold>)
>>

introspection-cond-term-constraint(t, motion_id) ::= <<
motion_spec::runtime::constraint_satisfied(shared.<t.error_id>)
>>

introspection-cond-term-flag(t, motion_id) ::= <<
<t.motion_id>_state_instance.<t.flag>
>>

introspection-cond-term-event(t, motion_id) ::= <<
<t.motion_id>_state_instance.<t.monitor_id>_event_triggered
>>

introspection-bool-condition(terms, any, motion_id) ::= <<
<if(rest(terms))>(<if(any)><terms:{t | <introspection-cond-term(t, motion_id)>}; separator=" || "><else><terms:{t | <introspection-cond-term(t, motion_id)>}; separator=" && "><endif>)<else><introspection-cond-term(first(terms), motion_id)><endif>
>>

introspection-monitor-sample(slot) ::= <<
<if(slot.has_active)> pub.monitor_sample(<slot.uri>, <if(slot.active_terms_present)><bool-condition(slot.active_terms, slot.active_any)><else>false<endif>, <if(slot.active_terms_present)><bool-condition(slot.active_terms, slot.active_any)><else>false<endif>);<else> pub.monitor_sample(<slot.uri>, <if(slot.composite_error)>motion_spec::runtime::constraint_error_value(<slot.value_expr>)<else><slot.value_expr><endif>, motion_spec::runtime::constraint_satisfied(<slot.value_expr>));<endif>
<if(slot.has_active)> pub.monitor_sample(<slot.uri>, <if(slot.active_terms_present)><introspection-bool-condition(slot.active_terms, slot.active_any, slot.motion)><else>false<endif>, <if(slot.active_terms_present)><introspection-bool-condition(slot.active_terms, slot.active_any, slot.motion)><else>false<endif>);<else> pub.monitor_sample(<slot.uri>, <if(slot.composite_error)>motion_spec::runtime::constraint_error_value(<slot.value_expr>)<else><slot.value_expr><endif>, motion_spec::runtime::constraint_satisfied(<slot.value_expr>));<endif>
>>

introspection-quantity-sample(slot, views) ::= <<
Expand Down
64 changes: 43 additions & 21 deletions code-generator/main.stg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,27 +102,32 @@ app-main-base-setup() ::= <<
};
>>

app-arm-includes(backend, has_arm) ::= <<
<({app-arm-includes-<backend>})(has_arm)>
app-arm-includes(backend, has_arm, wrench_outputs) ::= <<

@vamsikalagaturu vamsikalagaturu Jul 24, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

all these changes with passing wrench_outputs as an arg to methods have to go. not a right implementation. for eg. its being passed to mujoco templates also even though not being used. and a very specific case for wrench. this should be coming from sensors.

<({app-arm-includes-<backend>})(has_arm, wrench_outputs)>
>>

app-arm-setup(backend, arm_solvers, scene) ::= <<
<({app-arm-setup-<backend>})(arm_solvers, scene)>
app-arm-setup(backend, arm_solvers, scene, wrench_outputs) ::= <<
<({app-arm-setup-<backend>})(arm_solvers, scene, wrench_outputs)>
>>

app-loop-condition(backend, has_arm) ::= <<
<({app-loop-condition-<backend>})(has_arm)>
>>

app-arm-cleanup(backend, has_arm) ::= <<
<({app-arm-cleanup-<backend>})(has_arm)>
app-arm-cleanup(backend, has_arm, wrench_outputs) ::= <<
<({app-arm-cleanup-<backend>})(has_arm, wrench_outputs)>
>>

app-arm-post-setup(backend, arm_solvers, motions) ::= <<
<({app-arm-post-setup-<backend>})(arm_solvers, motions)>
app-arm-start(backend, arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx) ::= <<

@vamsikalagaturu vamsikalagaturu Jul 24, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why fsm specifics being passed??

<({app-arm-start-<backend>})(arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx)>
>>

app-arm-post-setup-robif2b(arm_solvers, motions) ::= <<
app-arm-post-setup(backend, arm_solvers, motions, wrench_outputs) ::= <<
<({app-arm-post-setup-<backend>})(arm_solvers, motions, wrench_outputs)>
>>

app-arm-post-setup-robif2b(arm_solvers, motions, wrench_outputs) ::= <<
<robif2b-hardware-post-setup(arm_solvers, wrench_outputs)>
>>

app-arm-loop-reset(backend, arm_solvers, fsm_namespace) ::= <<
Expand All @@ -132,8 +137,8 @@ app-arm-loop-reset(backend, arm_solvers, fsm_namespace) ::= <<
app-arm-loop-reset-robif2b(arm_solvers, fsm_namespace) ::= <<
>>

app-arm-runtime-step(backend, arm_solvers) ::= <<
<({app-arm-runtime-step-<backend>})(arm_solvers)>
app-arm-runtime-step(backend, arm_solvers, wrench_outputs) ::= <<
<({app-arm-runtime-step-<backend>})(arm_solvers, wrench_outputs)>
>>

clock-time-source(backend, arm_solvers, has_arm) ::= <<
Expand Down Expand Up @@ -194,7 +199,7 @@ app_main(motions, wrench_outputs, has_arm, has_mobile_base, shared_schedule, clo
<endif>
<motions:{motion | <motion-include(motion)>}; separator="\n">

<app-arm-includes(backend, has_arm)>
<app-arm-includes(backend, has_arm, wrench_outputs)>
#include \<unistd.h\>

int main(int argc, char **argv) {
Expand All @@ -206,7 +211,7 @@ int main(int argc, char **argv) {
<endif>
<if(has_arm)>

<app-arm-setup(backend, arm_solvers, scene)>
<app-arm-setup(backend, arm_solvers, scene, wrench_outputs)>
<endif>
<wrench_outputs:{wrench | <wrench-measurement(wrench)>}; separator="\n">
<wrench_outputs:{wrench | robot.<wrench.id> = &<wrench.id>_measurement;}; separator="\n">
Expand Down Expand Up @@ -243,14 +248,14 @@ int main(int argc, char **argv) {
<endif>
<if(has_arm)>
<app-arm-trace-update(backend, arm_solvers, trace)>
<app-arm-runtime-step(backend, arm_solvers)>
<app-arm-runtime-step(backend, arm_solvers, wrench_outputs)>
<endif>

motion_spec::runtime::sleep_until_next(_next_tick, <control_period_ns>);
}
<if(has_arm)>

<app-arm-cleanup(backend, has_arm)>
<app-arm-cleanup(backend, has_arm, wrench_outputs)>
<arm_solvers:{solver |
<robot-shutdown(solver, backend)>
}; separator="\n">
Expand All @@ -271,6 +276,9 @@ static void step_<motion.id>(<motion.id>_state &<motion.id>_state_instance, shar
if (!<motion.id>_state_instance.active) {
<motion.id>_state_instance.active = true;
<motion.id>_state_instance.active_steps = 0;
<if(motion.has_active_elapsed)> shared.<motion.id>_motion_start_time = shared.clock_time_s;
<motion.id>_state_instance.motion_start_time = shared.clock_time_s;
<endif>
<motion.controllers:{controller | <controller-reset(motion.id, controller)>}; separator="\n">
}
update_<motion.id>(<motion.id>_state_instance, shared, robot);
Expand Down Expand Up @@ -310,7 +318,7 @@ ref_main(motions, wrench_outputs, has_arm, has_mobile_base, shared_schedule, clo
<endif>
<motions:{motion | <motion-include(motion)>}; separator="\n">

<app-arm-includes(backend, has_arm)>
<app-arm-includes(backend, has_arm, wrench_outputs)>
#include \<unistd.h\>
<if(fsm_namespace)>

Expand All @@ -330,7 +338,7 @@ int main(int argc, char **argv) {
<endif>
<if(has_arm)>

<app-arm-setup(backend, arm_solvers, scene)>
<app-arm-setup(backend, arm_solvers, scene, wrench_outputs)>
<endif>
<wrench_outputs:{wrench | <wrench-measurement(wrench)>}; separator="\n">
<wrench_outputs:{wrench | robot.<wrench.id> = &<wrench.id>_measurement;}; separator="\n">
Expand All @@ -350,7 +358,7 @@ int main(int argc, char **argv) {
<endif>

<if(has_arm)>
<app-arm-post-setup(backend, arm_solvers, motions)>
<app-arm-post-setup(backend, arm_solvers, motions, wrench_outputs)>
<endif>

<if(fsm_namespace)>
Expand All @@ -360,6 +368,13 @@ int main(int argc, char **argv) {

<fsm-dispatch(motions, fsm_namespace)>

<endif>
<if(has_arm)>
<if(needs_clock_time)> // Initialize the runtime clock before FSM priming so active-elapsed
// motions snapshot their start time in the same clock domain as the main loop.
shared.clock_time_s = <clock-time-source(backend, arm_solvers, has_arm)>;
<endif>
<app-arm-start(backend, arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx)>
<endif>
int _step = 0;
<if(fsm_namespace)>
Expand Down Expand Up @@ -402,6 +417,8 @@ int main(int argc, char **argv) {
if (!<motion.id>_state_instance.active && can_start_<motion.id>(<sig-args(motion.id, motion.can_start_needs_state, motion.can_start_needs_shared, motion.can_start_needs_robot)>)) {
<motion.id>_state_instance.active = true;
<motion.id>_state_instance.active_steps = 0;
<if(motion.has_active_elapsed)> shared.<motion.id>_motion_start_time = shared.clock_time_s;
<endif>
<if(motion.has_active_elapsed)> <motion.id>_state_instance.motion_start_time = shared.clock_time_s;
<endif>
<motion.until_monitors:{m | <motion.id>_state_instance.<m.id>_previous = false;
Expand Down Expand Up @@ -435,7 +452,7 @@ int main(int argc, char **argv) {
<endif>
<if(has_arm)>
<app-arm-trace-update(backend, arm_solvers, trace)>
<app-arm-runtime-step(backend, arm_solvers)>
<app-arm-runtime-step(backend, arm_solvers, wrench_outputs)>
<endif>
<if(fsm_namespace)>
fsm_step_nbx(fsm);
Expand All @@ -455,10 +472,13 @@ int main(int argc, char **argv) {
++_step;
motion_spec::runtime::sleep_until_next(_next_tick, <control_period_ns>);
}
#ifdef MOTION_SPEC_ENABLE_INTROSPECTION
_introspection_pub.close();
#endif
<app-headless-telemetry(backend, shared_data, arm_solvers, scene)>
<if(has_arm)>

<app-arm-cleanup(backend, has_arm)>
<app-arm-cleanup(backend, has_arm, wrench_outputs)>
<arm_solvers:{solver |
<robot-shutdown(solver, backend)>
}; separator="\n">
Expand All @@ -480,7 +500,7 @@ int main(int argc, char **argv) {
}
>>

shared_state_header(shared_data, wrench_outputs, has_arm, has_mobile_base, arm_solvers, base_velocity_solvers, base_force_solvers, backend, fsm_namespace, fsm_header, has_ros, ros_publishers) ::= <<
shared_state_header(shared_data, wrench_outputs, has_arm, has_mobile_base, arm_solvers, base_velocity_solvers, base_force_solvers, backend, fsm_namespace, fsm_header, motions, has_ros, ros_publishers) ::= <<
#pragma once

#include "frame_layout.h"
Expand Down Expand Up @@ -650,6 +670,8 @@ struct shared_data {
++_motion_spec_event_count;
\}
double clock_time_s = 0.0; // runtime clock seconds; MuJoCo sim time or real monotonic time
<motions:{motion | <if(motion.has_active_elapsed)> double <motion.id>_motion_start_time = -1.0;
<endif>}; separator="\n">
<shared_data:{var | <indented-shared-member(var)>}; separator="\n">
};
>>
16 changes: 10 additions & 6 deletions code-generator/mj_kdl_backend.stg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ robot-shutdown-mj_kdl-KinovaGen3(solver) ::= <<
<endif>
>>

app-arm-includes-mj_kdl(has_arm) ::= <<
app-arm-includes-mj_kdl(has_arm, wrench_outputs) ::= <<

@vamsikalagaturu vamsikalagaturu Jul 24, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

commented above

<if(has_arm)>
#ifdef MOTION_SPEC_ENABLE_INTROSPECTION
#include "introspect_model.hpp"
Expand All @@ -71,7 +71,7 @@ app-arm-includes-mj_kdl(has_arm) ::= <<
<endif>
>>

app-arm-setup-mj_kdl(arm_solvers, scene) ::= <<
app-arm-setup-mj_kdl(arm_solvers, scene, wrench_outputs) ::= <<
bool headless = false;
int headless_steps = 2000;
for (int i = 1; i \< argc; ++i) {
Expand Down Expand Up @@ -258,7 +258,7 @@ app-arm-setup-mj_kdl(arm_solvers, scene) ::= <<
}
>>

app-arm-post-setup-mj_kdl(arm_solvers, motions) ::= <<
app-arm-post-setup-mj_kdl(arm_solvers, motions, wrench_outputs) ::= <<
mj_env.on_reset = [&](mj_kdl::ResetContext *) {
const double _q_home[7] = {0.0, 0.2618, 3.1416, -2.2689, 0.0, 0.9599, 1.5708};
<arm_solvers:{solver | <if(solver.runtime_owner)> {
Expand Down Expand Up @@ -290,18 +290,22 @@ app-loop-condition-mj_kdl(has_arm) ::= <<
(!headless || headless_steps-- > 0)
>>

app-arm-cleanup-mj_kdl(has_arm) ::= <<
app-arm-cleanup-mj_kdl(has_arm, wrench_outputs) ::= <<
<if(has_arm)> if (!headless) {
mj_kdl::cleanup(&mj_viewer);
}
mj_kdl::cleanup(&mj_env);<endif>
>>

app-arm-runtime-step-mj_kdl(arm_solvers) ::= <<
app-arm-start-mj_kdl(arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx) ::= <<
>>

app-arm-runtime-step-mj_kdl(arm_solvers, wrench_outputs) ::= <<
<arm_solvers:{solver | <if(solver.runtime_owner)> mj_kdl::update(&mj_<solver.runtime_id>);
<endif>}; separator="">
if (!mj_kdl::step(&mj_<first(arm_solvers).runtime_id>)) {
std::exit(0);
std::cerr \<\< "mj_kdl viewer closed\n";
break;
}
>>

Expand Down
Loading
Loading