Skip to content

Commit a1410a9

Browse files
johnwasonjohnwason
committed
Update to upstream changes in simple planner
1 parent 0466e23 commit a1410a9

File tree

4 files changed

+35
-32
lines changed

4 files changed

+35
-32
lines changed

‎tesseract_python/swig/tesseract_motion_planners_simple_python.i‎

Lines changed: 8 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -35,13 +35,11 @@
3535

3636
%{
3737
// tesseract_motion_planners_simple
38+
#include <tesseract_motion_planners/simple/profile/simple_planner_utils.h>
3839
#include <tesseract_motion_planners/simple/profile/simple_planner_profile.h>
39-
#include <tesseract_motion_planners/simple/profile/simple_planner_default_plan_profile.h>
40-
#include <tesseract_motion_planners/simple/profile/simple_planner_default_lvs_plan_profile.h>
41-
#include <tesseract_motion_planners/simple/profile/simple_planner_interpolation_plan_profile.h>
42-
#include <tesseract_motion_planners/simple/step_generators/fixed_size_assign_position.h>
43-
#include <tesseract_motion_planners/simple/step_generators/fixed_size_interpolation.h>
44-
#include <tesseract_motion_planners/simple/step_generators/lvs_interpolation.h>
40+
#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h>
41+
#include <tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h>
42+
#include <tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h>
4543
#include <tesseract_motion_planners/simple/simple_motion_planner.h>
4644

4745
#include <tesseract_common/status_code.h>
@@ -52,22 +50,11 @@
5250
#include <tesseract_kinematics/core/rop_inverse_kinematics.h>
5351
%}
5452

55-
%tesseract_std_function_base(JointJointStepGenerator,tesseract_planning,tesseract_planning::CompositeInstruction,const tesseract_planning::JointWaypoint&,a,const tesseract_planning::JointWaypoint&,b,const tesseract_planning::PlanInstruction&,c,const tesseract_planning::PlannerRequest&,d,const tesseract_planning::ManipulatorInfo&,e);
56-
%tesseract_std_function(JointJointStepGenerator,tesseract_planning,tesseract_planning::CompositeInstruction,const tesseract_planning::JointWaypoint&,a,const tesseract_planning::JointWaypoint&,b,const tesseract_planning::PlanInstruction&,c,const tesseract_planning::PlannerRequest&,d,const tesseract_planning::ManipulatorInfo&,e);
57-
%tesseract_std_function_base(JointCartStepGenerator,tesseract_planning,tesseract_planning::CompositeInstruction,const tesseract_planning::JointWaypoint&,a,const tesseract_planning::CartesianWaypoint&,b,const tesseract_planning::PlanInstruction&,c,const tesseract_planning::PlannerRequest&,d,const tesseract_planning::ManipulatorInfo&,e);
58-
%tesseract_std_function(JointCartStepGenerator,tesseract_planning,tesseract_planning::CompositeInstruction,const tesseract_planning::JointWaypoint&,a,const tesseract_planning::CartesianWaypoint&,b,const tesseract_planning::PlanInstruction&,c,const tesseract_planning::PlannerRequest&,d,const tesseract_planning::ManipulatorInfo&,e);
59-
%tesseract_std_function_base(CartJointStepGenerator,tesseract_planning,tesseract_planning::CompositeInstruction,const tesseract_planning::CartesianWaypoint&,a,const tesseract_planning::JointWaypoint&,b,const tesseract_planning::PlanInstruction&,c,const tesseract_planning::PlannerRequest&,d,const tesseract_planning::ManipulatorInfo&,e);
60-
%tesseract_std_function(CartJointStepGenerator,tesseract_planning,tesseract_planning::CompositeInstruction,const tesseract_planning::CartesianWaypoint&,a,const tesseract_planning::JointWaypoint&,b,const tesseract_planning::PlanInstruction&,c,const tesseract_planning::PlannerRequest&,d,const tesseract_planning::ManipulatorInfo&,e);
61-
%tesseract_std_function_base(CartCartStepGenerator,tesseract_planning,tesseract_planning::CompositeInstruction,const tesseract_planning::CartesianWaypoint&,a,const tesseract_planning::CartesianWaypoint&,b,const tesseract_planning::PlanInstruction&,c,const tesseract_planning::PlannerRequest&,d,const tesseract_planning::ManipulatorInfo&,e);
62-
%tesseract_std_function(CartCartStepGenerator,tesseract_planning,tesseract_planning::CompositeInstruction,const tesseract_planning::CartesianWaypoint&,a,const tesseract_planning::CartesianWaypoint&,b,const tesseract_planning::PlanInstruction&,c,const tesseract_planning::PlannerRequest&,d,const tesseract_planning::ManipulatorInfo&,e);
63-
6453
// tesseract_motion_planners_simple
6554
#define TESSERACT_MOTION_PLANNERS_SIMPLE_PUBLIC
55+
%include "tesseract_motion_planners/simple/profile/simple_planner_utils.h"
6656
%include "tesseract_motion_planners/simple/profile/simple_planner_profile.h"
67-
%include "tesseract_motion_planners/simple/profile/simple_planner_default_lvs_plan_profile.h"
68-
%include "tesseract_motion_planners/simple/profile/simple_planner_interpolation_plan_profile.h"
69-
%include "tesseract_motion_planners/simple/profile/simple_planner_default_plan_profile.h"
70-
%include "tesseract_motion_planners/simple/step_generators/fixed_size_assign_position.h"
71-
%include "tesseract_motion_planners/simple/step_generators/fixed_size_interpolation.h"
72-
%include "tesseract_motion_planners/simple/step_generators/lvs_interpolation.h"
57+
%include "tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h"
58+
%include "tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h"
59+
%include "tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h"
7360
%include "tesseract_motion_planners/simple/simple_motion_planner.h"

‎tesseract_python/swig/tesseract_process_managers_python.i‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -42,13 +42,11 @@
4242
#include <tesseract_motion_planners/core/profile_dictionary.h>
4343

4444
// tesseract_motion_planners_simple
45+
#include <tesseract_motion_planners/simple/profile/simple_planner_utils.h>
4546
#include <tesseract_motion_planners/simple/profile/simple_planner_profile.h>
46-
#include <tesseract_motion_planners/simple/profile/simple_planner_default_plan_profile.h>
47-
#include <tesseract_motion_planners/simple/profile/simple_planner_default_lvs_plan_profile.h>
48-
#include <tesseract_motion_planners/simple/profile/simple_planner_interpolation_plan_profile.h>
49-
#include <tesseract_motion_planners/simple/step_generators/fixed_size_assign_position.h>
50-
#include <tesseract_motion_planners/simple/step_generators/fixed_size_interpolation.h>
51-
#include <tesseract_motion_planners/simple/step_generators/lvs_interpolation.h>
47+
#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h>
48+
#include <tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h>
49+
#include <tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h>
5250
#include <tesseract_motion_planners/simple/simple_motion_planner.h>
5351

5452
// tesseract_motion_planners_trajopt

‎tesseract_python/swig/tesseract_visualization_python.i‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,14 @@
3535

3636
%{
3737
// tesseract_visualization
38+
#include <tesseract_visualization/markers/marker.h>
39+
#include <tesseract_visualization/markers/arrow_marker.h>
40+
#include <tesseract_visualization/markers/axis_marker.h>
41+
#include <tesseract_visualization/markers/contact_results_marker.h>
42+
#include <tesseract_visualization/markers/geometry_marker.h>
43+
#include <tesseract_visualization/markers/toolpath_marker.h>
3844
#include <tesseract_visualization/visualization.h>
45+
#include <tesseract_visualization/visualization_loader.h>
3946

4047
#include <tesseract_common/status_code.h>
4148
#include <tesseract_common/resource.h>
@@ -47,6 +54,14 @@
4754

4855
// tesseract_visualization
4956
#define TESSERACT_VISUALIZATION_PUBLIC
57+
58+
%include "tesseract_visualization/markers/marker.h"
59+
%include "tesseract_visualization/markers/arrow_marker.h"
60+
%include "tesseract_visualization/markers/axis_marker.h"
61+
%include "tesseract_visualization/markers/contact_results_marker.h"
62+
%include "tesseract_visualization/markers/geometry_marker.h"
63+
%include "tesseract_visualization/markers/toolpath_marker.h"
5064
%include "tesseract_visualization/visualization.h"
65+
%include "tesseract_visualization/visualization_loader.h"
5166

5267

‎tesseract_python/tests/tesseract_motion_planning/test_simple_planner.py‎

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -7,9 +7,10 @@
77
from tesseract.tesseract_environment import Environment
88
from tesseract.tesseract_common import FilesystemPath, ManipulatorInfo
99
from tesseract.tesseract_command_language import JointWaypoint, CartesianWaypoint, Waypoint, \
10-
PlanInstructionType_FREESPACE, PlanInstruction, Instruction, isMoveInstruction, isStateWaypoint
10+
PlanInstructionType_FREESPACE, PlanInstructionType_START, PlanInstruction, Instruction, \
11+
isMoveInstruction, isStateWaypoint
1112
from tesseract.tesseract_motion_planners import PlannerRequest
12-
from tesseract.tesseract_motion_planners_simple import LVSInterpolateStateWaypoint
13+
from tesseract.tesseract_motion_planners_simple import SimplePlannerLVSPlanProfile
1314

1415
def _locate_resource(url):
1516
try:
@@ -49,14 +50,16 @@ def test_interpolatestatewaypoint_jointcart_freespace():
4950
fwd_kin = env.getManipulatorManager().getFwdKinematicSolver(manip_info.manipulator)
5051
wp1 = JointWaypoint(joint_names, np.zeros((7,),dtype=np.float64))
5152
wp2 = CartesianWaypoint(fwd_kin.calcFwdKin(np.ones((7,),dtype=np.float64))[1])
52-
instr = PlanInstruction(Waypoint(wp1), PlanInstructionType_FREESPACE, "TEST_PROFILE", manip_info)
53+
instr1 = PlanInstruction(Waypoint(wp1), PlanInstructionType_START, "TEST_PROFILE", manip_info)
54+
instr2 = PlanInstruction(Waypoint(wp2), PlanInstructionType_FREESPACE, "TEST_PROFILE", manip_info)
5355

54-
composite = LVSInterpolateStateWaypoint(wp1,wp2,instr,request,ManipulatorInfo(),3.14,0.5,1.57,5)
56+
profile = SimplePlannerLVSPlanProfile(3.14,0.5,1.57,5)
57+
composite = profile.generate(instr1,instr2,request,ManipulatorInfo())
5558

5659
for c in composite:
5760
assert isMoveInstruction(c)
5861
assert isStateWaypoint(c.cast_MoveInstruction().getWaypoint())
59-
assert c.cast_MoveInstruction().getProfile() == instr.getProfile()
62+
assert c.cast_MoveInstruction().getProfile() == instr2.getProfile()
6063

6164
mi = composite[-1].cast_const_MoveInstruction()
6265
last_position = mi.getWaypoint().cast_const_StateWaypoint().position

0 commit comments

Comments
 (0)