Skip to content

Commit bcbff64

Browse files
urfeexurfeex
committed
Support q-near
1 parent fc108f4 commit bcbff64

6 files changed

Lines changed: 118 additions & 29 deletions

File tree

‎examples/instruction_executor.cpp‎

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -104,9 +104,17 @@ int main(int argc, char* argv[])
104104

105105
// acceleration / velocity parametrization brace-init style will be interpreted as joint
106106
// positions
107-
instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0);
107+
instruction_executor->moveJ({ -1.742, -1.726, -2.214, -0.773, 1.572, -0.171 }, 2.0, 2.0);
108108

109-
// goal time parametrization -- acceleration and velocity will be ignored
109+
// Passing a pose to moveJ will make it internally solve inverse kinematics.
110+
instruction_executor->moveJ(urcl::Pose{ -0.206, -0.6437, 0.202, 0.0, 3.140, 0.0 }, 2.0, 2.0);
111+
112+
// To provide a q_near hint for the IK solver a joint configuration near the target can be added to a pose.
113+
urcl::Pose target_pose{ -0.206, -0.6437, 0.202, 0.0, 3.140, 0.0 };
114+
target_pose.q_near = urcl::Q{ -1.7, -4, 1.5, -1, 1.5, 0 };
115+
instruction_executor->moveJ(target_pose);
116+
117+
// goal time parametrization -- acceleration and velocity will be scaled to meed the goal time.
110118
instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, 0.1, goal_time_sec);
111119

112120
// moveL calls with brace-init style is interpreted as a pose

‎include/ur_client_library/control/motion_primitives.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -175,6 +175,8 @@ class MoveLPrimitive : public MotionPrimitiveWithTarget
175175
const std::chrono::duration<double> duration = std::chrono::milliseconds(0),
176176
const double acceleration = 1.4, const double velocity = 1.04);
177177

178+
~MoveLPrimitive() override;
179+
178180
void setTarget(const MotionTarget& target) override;
179181

180182
[[deprecated("Use getTarget() and setTarget() instead.")]]
@@ -196,6 +198,8 @@ class MovePPrimitive : public MotionPrimitiveWithTarget
196198
MovePPrimitive(const MotionTarget& target, const double blend_radius = 0, const double acceleration = 1.4,
197199
const double velocity = 1.04);
198200

201+
~MovePPrimitive() override;
202+
199203
void setTarget(const MotionTarget& target) override;
200204

201205
[[deprecated("Use getTarget() and setTarget() instead.")]]
@@ -220,6 +224,8 @@ class MoveCPrimitive : public MotionPrimitiveWithTarget
220224
MoveCPrimitive(const MotionTarget& via_point, const MotionTarget& target, const double blend_radius = 0,
221225
const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0);
222226

227+
~MoveCPrimitive() override;
228+
223229
void setTarget(const MotionTarget& target) override;
224230

225231
void setVia(const MotionTarget& via_point);
@@ -313,6 +319,8 @@ class OptimoveLPrimitive : public MotionPrimitiveWithTarget
313319
OptimoveLPrimitive(const MotionTarget& target, const double blend_radius = 0,
314320
const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3);
315321

322+
~OptimoveLPrimitive() override;
323+
316324
void setTarget(const MotionTarget& target) override;
317325

318326
bool validate() const override;

‎include/ur_client_library/types.h‎

Lines changed: 28 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -21,9 +21,11 @@
2121
#pragma once
2222

2323
#include <inttypes.h>
24+
#include <algorithm>
2425
#include <array>
2526
#include <functional>
2627
#include <iostream>
28+
#include <optional>
2729
#include <variant>
2830
#include "ur_client_library/log.h"
2931

@@ -60,13 +62,18 @@ struct Q
6062
std::vector<double> values;
6163
};
6264

65+
inline bool operator==(const Q& lhs, const Q& rhs)
66+
{
67+
return lhs.values.size() == rhs.values.size() && std::equal(lhs.values.begin(), lhs.values.end(), rhs.values.begin());
68+
}
69+
6370
struct Pose
6471
{
65-
Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0)
72+
Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0), q_near(std::nullopt)
6673
{
6774
}
6875
Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz)
69-
: x(x), y(y), z(z), rx(rx), ry(ry), rz(rz)
76+
: x(x), y(y), z(z), rx(rx), ry(ry), rz(rz), q_near(std::nullopt)
7077
{
7178
}
7279
double x;
@@ -76,9 +83,27 @@ struct Pose
7683
double ry;
7784
double rz;
7885

86+
/*!
87+
* Optional joint-space hint (six joint positions in radians) passed to the controller for inverse
88+
* kinematics when this pose is used as a Cartesian motion target over the trajectory interface.
89+
*/
90+
std::optional<Q> q_near;
91+
7992
bool operator==(const Pose& other) const
8093
{
81-
return x == other.x && y == other.y && z == other.z && rx == other.rx && ry == other.ry && rz == other.rz;
94+
if (x != other.x || y != other.y || z != other.z || rx != other.rx || ry != other.ry || rz != other.rz)
95+
{
96+
return false;
97+
}
98+
if (q_near.has_value() != other.q_near.has_value())
99+
{
100+
return false;
101+
}
102+
if (!q_near.has_value())
103+
{
104+
return true;
105+
}
106+
return *q_near == *other.q_near;
82107
}
83108
};
84109

‎resources/external_control.urscript‎

Lines changed: 28 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -565,36 +565,42 @@ thread trajectoryThread():
565565
blend_radius = 0.0
566566
is_last_point = True
567567
end
568+
569+
second_block = [raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate
570+
velocity = raw_point[13] / MULT_jointstate
571+
acceleration = raw_point[14] / MULT_jointstate
572+
has_qnear = raw_point[15] / MULT_jointstate
573+
568574
# MoveJ point
569575
if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEJ:
570-
acceleration = raw_point[13] / MULT_jointstate
571-
velocity = raw_point[7] / MULT_jointstate
576+
textmsg("Moving to joint position: ", q)
572577
movej(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius)
573578

574579
# reset old acceleration
575580
spline_qdd = [0, 0, 0, 0, 0, 0]
576581
spline_qd = [0, 0, 0, 0, 0, 0]
577582
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEJ_POSE:
578-
acceleration = raw_point[13] / MULT_jointstate
579-
velocity = raw_point[7] / MULT_jointstate
580-
movej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius)
583+
target_pose = p[q[0], q[1], q[2], q[3], q[4], q[5]]
584+
textmsg("Moving to pose: ", target_pose)
585+
target_q = get_inverse_kin(target_pose, get_target_joint_positions())
586+
if has_qnear:
587+
target_q = get_inverse_kin(target_pose, second_block)
588+
end
589+
movej(target_q, a = acceleration, v = velocity, t = tmptime, r = blend_radius)
590+
581591

582592
# reset old acceleration
583593
spline_qdd = [0, 0, 0, 0, 0, 0]
584594
spline_qd = [0, 0, 0, 0, 0, 0]
585595

586596
# Movel point
587597
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL:
588-
acceleration = raw_point[13] / MULT_jointstate
589-
velocity = raw_point[7] / MULT_jointstate
590598
movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius)
591599

592600
# reset old acceleration
593601
spline_qdd = [0, 0, 0, 0, 0, 0]
594602
spline_qd = [0, 0, 0, 0, 0, 0]
595603
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL_JOINT:
596-
acceleration = raw_point[13] / MULT_jointstate
597-
velocity = raw_point[7] / MULT_jointstate
598604
movel(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius)
599605

600606
# reset old acceleration
@@ -603,16 +609,12 @@ thread trajectoryThread():
603609

604610
# MoveP point
605611
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEP:
606-
acceleration = raw_point[13] / MULT_jointstate
607-
velocity = raw_point[7] / MULT_jointstate
608612
movep(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
609613

610614
# reset old acceleration
611615
spline_qdd = [0, 0, 0, 0, 0, 0]
612616
spline_qd = [0, 0, 0, 0, 0, 0]
613617
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEP_JOINT:
614-
acceleration = raw_point[13] / MULT_jointstate
615-
velocity = raw_point[7] / MULT_jointstate
616618
movep(q, a = acceleration, v = velocity, r = blend_radius)
617619

618620
# reset old acceleration
@@ -623,7 +625,7 @@ thread trajectoryThread():
623625
raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT or
624626
raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_POSE_JOINT or
625627
raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT_POSE:
626-
local v = [raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate
628+
local v = second_block
627629
if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC:
628630
via = p[v[0], v[1], v[2], v[3], v[4], v[5]]
629631
target = p[q[0], q[1], q[2], q[3], q[4], q[5]]
@@ -638,8 +640,6 @@ thread trajectoryThread():
638640
target = q
639641
end
640642

641-
velocity = raw_point[13] / MULT_jointstate
642-
acceleration = raw_point[14] / MULT_jointstate
643643
mode = raw_point[15] / MULT_jointstate
644644
movec(via, target, acceleration, velocity, blend_radius, mode)
645645

@@ -673,20 +673,28 @@ thread trajectoryThread():
673673

674674
# OptimoveJ point
675675
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ_POSE:
676-
acceleration = raw_point[13] / MULT_jointstate
677-
velocity = raw_point[7] / MULT_jointstate
678676
{% if ROBOT_SOFTWARE_VERSION >= v5.21.0 %}
679677
{% if ROBOT_SOFTWARE_VERSION < v6.0.0 %}
680678
if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ:
681679
optimovej(q, a = acceleration, v = velocity, r = blend_radius)
682680
else:
683-
optimovej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
681+
if has_qnear:
682+
target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], second_block)
683+
else:
684+
target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], get_target_joint_positions())
685+
end
686+
optimovej(target_q, a = acceleration, v = velocity, r = blend_radius)
684687
end
685688
{% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %}
686689
if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ:
687690
optimovej(q, a = acceleration, v = velocity, r = blend_radius)
688691
else:
689-
optimovej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
692+
if has_qnear:
693+
target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], second_block)
694+
else:
695+
target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], get_target_joint_positions())
696+
end
697+
optimovej(target_q, a = acceleration, v = velocity, r = blend_radius)
690698
end
691699
{% else %}
692700
popup("Optimovej is only supported from software 10.8.0 and upwards.", error=True, blocking=False)
@@ -709,8 +717,6 @@ thread trajectoryThread():
709717

710718
# OptimoveL point
711719
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL_JOINT:
712-
acceleration = raw_point[13] / MULT_jointstate
713-
velocity = raw_point[7] / MULT_jointstate
714720

715721
{% if ROBOT_SOFTWARE_VERSION >= v5.21.0 %}
716722
{% if ROBOT_SOFTWARE_VERSION < v6.0.0 %}

‎src/control/motion_primitives.cpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -191,6 +191,11 @@ MoveLPrimitive::MoveLPrimitive(const MotionTarget& target, const double blend_ra
191191
{
192192
setTarget(target);
193193
}
194+
195+
MoveLPrimitive::~MoveLPrimitive()
196+
{
197+
}
198+
194199
void MoveLPrimitive::setTarget(const MotionTarget& target)
195200
{
196201
std::visit(
@@ -225,6 +230,10 @@ MovePPrimitive::MovePPrimitive(const MotionTarget& target, const double blend_ra
225230
setTarget(target);
226231
}
227232

233+
MovePPrimitive::~MovePPrimitive()
234+
{
235+
}
236+
228237
void MovePPrimitive::setTarget(const MotionTarget& target)
229238
{
230239
std::visit(
@@ -265,6 +274,10 @@ MoveCPrimitive::MoveCPrimitive(const MotionTarget& via_point, const MotionTarget
265274
setTarget(target);
266275
}
267276

277+
MoveCPrimitive::~MoveCPrimitive()
278+
{
279+
}
280+
268281
void MoveCPrimitive::setTarget(const MotionTarget& target)
269282
{
270283
target_ = std::make_unique<MotionTarget>(target);
@@ -390,6 +403,10 @@ OptimoveLPrimitive::OptimoveLPrimitive(const MotionTarget& target, const double
390403
setTarget(target);
391404
}
392405

406+
OptimoveLPrimitive::~OptimoveLPrimitive()
407+
{
408+
}
409+
393410
void OptimoveLPrimitive::setTarget(const MotionTarget& target)
394411
{
395412
std::visit(

‎src/control/trajectory_point_interface.cpp‎

Lines changed: 27 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -123,8 +123,33 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
123123
{
124124
auto with_target = std::static_pointer_cast<control::MotionPrimitiveWithTarget>(primitive);
125125
first_block = motionTargetToBlock(with_target->getTarget().value());
126-
second_block.fill(primitive->velocity);
127-
third_block.fill(primitive->acceleration);
126+
bool has_qnear = false;
127+
second_block = std::visit(
128+
[&has_qnear](const auto& alternative) -> vector6d_t {
129+
using T = std::decay_t<decltype(alternative)>;
130+
if constexpr (std::is_same_v<T, urcl::Pose>)
131+
{
132+
if (alternative.q_near.has_value())
133+
{
134+
has_qnear = true;
135+
return vector6d_t{ alternative.q_near->values[0], alternative.q_near->values[1],
136+
alternative.q_near->values[2], alternative.q_near->values[3],
137+
alternative.q_near->values[4], alternative.q_near->values[5] };
138+
}
139+
else
140+
{
141+
has_qnear = false;
142+
return vector6d_t{ 0, 0, 0, 0, 0, 0 };
143+
}
144+
}
145+
else
146+
{
147+
static_assert(std::is_same_v<T, urcl::Q>, "Unhandled MotionTarget alternative");
148+
return { 0, 0, 0, 0, 0, 0 };
149+
}
150+
},
151+
with_target->getTarget().value());
152+
third_block = { primitive->velocity, primitive->acceleration, static_cast<double>(has_qnear), 0, 0, 0 };
128153
break;
129154
}
130155
case MotionType::MOVEC:

0 commit comments

Comments
 (0)