Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
P
panda_mqtt_connector
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package registry
Container registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
This is an archived project. Repository and other project resources are read-only.
Show more breadcrumbs
CeTI
ROS
ROS Packages
panda_mqtt_connector
Commits
a7ebe96d
Commit
a7ebe96d
authored
4 years ago
by
Johannes Mey
Browse files
Options
Downloads
Patches
Plain Diff
straighten interface
parent
c511bf1f
Branches
Branches containing commit
No related tags found
No related merge requests found
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
config/config.yaml
+2
-1
2 additions, 1 deletion
config/config.yaml
src/TimedPlanner.cpp
+113
-124
113 additions, 124 deletions
src/TimedPlanner.cpp
src/util/TrajectoryUtil.cpp
+3
-26
3 additions, 26 deletions
src/util/TrajectoryUtil.cpp
src/util/TrajectoryUtil.h
+1
-4
1 addition, 4 deletions
src/util/TrajectoryUtil.h
with
119 additions
and
155 deletions
config/config.yaml
+
2
−
1
View file @
a7ebe96d
panda_mqtt_connector
:
server
:
"
tcp://localhost:1883"
robot_speed_factor
:
.
1
5
robot_speed_factor
:
.
9
5
robot_planning_mode
:
"
fluid_path"
# "fluid_path" or "cartesian_path"
default_trajectory
:
"
square"
# "square" or "circle", everything else is ignored
topics
:
robotConfig
:
"
robotconfig"
dataConfig
:
"
dataconfig"
...
...
This diff is collapsed.
Click to expand it.
src/TimedPlanner.cpp
+
113
−
124
View file @
a7ebe96d
...
...
@@ -18,41 +18,44 @@
namespace
PlannerState
{
std
::
vector
<
geometry_msgs
::
Pose
>
raw_trajectory
;
std
::
vector
<
geometry_msgs
::
Pose
>
raw_trajectory
;
TrajectoryUtil
traj_util
;
const
double
default_velocity
=
1.0
;
const
std
::
string
default_planning_mode
=
TrajectoryUtil
::
FLUID_PATH
;
const
bool
useCircle
=
true
;
bool
isLooping
=
true
;
}
void
constructPlate
(
std
::
vector
<
moveit_msgs
::
CollisionObject
>
&
collision_objects
,
double
x_dimension
,
double
y_dimension
)
{
void
constructPlate
(
std
::
vector
<
moveit_msgs
::
CollisionObject
>
&
collision_objects
,
double
x_dimension
,
double
y_dimension
)
{
moveit_msgs
::
CollisionObject
plate
;
moveit_msgs
::
CollisionObject
plate
;
plate
.
header
.
frame_id
=
"panda_link0"
;
plate
.
id
=
"plate"
;
plate
.
header
.
frame_id
=
"panda_link0"
;
plate
.
id
=
"plate"
;
plate
.
primitives
.
resize
(
1
);
plate
.
primitives
[
0
].
type
=
1u
;
plate
.
primitives
[
0
].
dimensions
.
resize
(
3
);
plate
.
primitives
[
0
].
dimensions
[
0
]
=
x_dimension
;
plate
.
primitives
[
0
].
dimensions
[
1
]
=
y_dimension
;
plate
.
primitives
[
0
].
dimensions
[
2
]
=
0.1
;
plate
.
primitives
.
resize
(
1
);
plate
.
primitives
[
0
].
type
=
1u
;
plate
.
primitives
[
0
].
dimensions
.
resize
(
3
);
plate
.
primitives
[
0
].
dimensions
[
0
]
=
x_dimension
;
plate
.
primitives
[
0
].
dimensions
[
1
]
=
y_dimension
;
plate
.
primitives
[
0
].
dimensions
[
2
]
=
0.1
;
plate
.
primitive_poses
.
resize
(
1
);
plate
.
primitive_poses
[
0
].
position
.
x
=
0
;
plate
.
primitive_poses
[
0
].
position
.
y
=
0
;
plate
.
primitive_poses
[
0
].
position
.
z
=
-
0.1
;
plate
.
primitive_poses
.
resize
(
1
);
plate
.
primitive_poses
[
0
].
position
.
x
=
0
;
plate
.
primitive_poses
[
0
].
position
.
y
=
0
;
plate
.
primitive_poses
[
0
].
position
.
z
=
-
0.1
;
plate
.
operation
=
plate
.
ADD
;
plate
.
operation
=
plate
.
ADD
;
collision_objects
.
push_back
(
plate
);
collision_objects
.
push_back
(
plate
);
}
void
initCircularTrajectory
(
double
radius
,
geometry_msgs
::
Point
origin
,
int
granularity
)
{
void
loadCircularTrajectory
(
std
::
vector
<
geometry_msgs
::
Pose
>
&
waypoints
,
double
radius
,
geometry_msgs
::
Point
origin
,
int
granularity
)
{
double
step_size_rad
=
2
*
M_PI
/
granularity
;
...
...
@@ -71,125 +74,68 @@ void initCircularTrajectory(double radius, geometry_msgs::Point origin, int gran
nextPose
.
position
.
y
=
origin
.
y
+
radius
*
cos
(
next_rad
);
nextPose
.
position
.
z
=
origin
.
z
;
PlannerState
::
raw_trajectory
.
push_back
(
nextPose
);
std
::
cout
<<
"added: "
<<
nextPose
<<
std
::
endl
;
waypoints
.
push_back
(
nextPose
);
}
}
void
initRawTrajectory
(
ros
::
NodeHandle
node_handle
,
moveit
::
planning_interface
::
MoveGroupInterface
*
group
,
bool
useDefault
,
bool
useCircularDefault
)
{
geometry_msgs
::
Pose
current_pose
=
group
->
getCurrentPose
().
pose
;
if
(
useCircularDefault
)
{
geometry_msgs
::
Point
origin
;
origin
.
z
=
.3
;
initCircularTrajectory
(
0.6
,
origin
,
20
);
return
;
}
if
(
!
PlannerState
::
traj_util
.
initWaypoints
(
PlannerState
::
raw_trajectory
,
node_handle
,
current_pose
)
&&
useDefault
)
{
// choose a default trajectory
geometry_msgs
::
Pose
target_pose_1
=
group
->
getCurrentPose
().
pose
;
geometry_msgs
::
Pose
target_pose_2
=
group
->
getCurrentPose
().
pose
;
geometry_msgs
::
Pose
target_pose_3
=
group
->
getCurrentPose
().
pose
;
geometry_msgs
::
Pose
target_pose_4
=
group
->
getCurrentPose
().
pose
;
target_pose_2
.
position
.
z
=
0.6
;
//0.583;
target_pose_2
.
position
.
y
=
-
0.6
;
//63;
target_pose_2
.
position
.
x
=
0
;
//-0.007;
PlannerState
::
raw_trajectory
.
push_back
(
target_pose_2
);
bool
loadWaypoints
(
std
::
vector
<
geometry_msgs
::
Pose
>
&
waypoints
,
const
ros
::
NodeHandle
&
node_handle
,
geometry_msgs
::
Pose
&
initialPose
)
{
target_pose_3
.
position
.
z
=
0.6
;
//0.691;
target_pose_3
.
position
.
y
=
-
0.032
;
target_pose_3
.
position
.
x
=
-
0.607
;
PlannerState
::
raw_trajectory
.
push_back
(
target_pose_3
);
// regardless if the loading fails, the trajectory is no longer "new" after calling this method
node_handle
.
setParam
(
"new_trajectory_available"
,
false
);
target_pose_4
.
position
.
z
=
0.6
;
target_pose_4
.
position
.
y
=
0.6
;
//0.509;
target_pose_4
.
position
.
x
=
0
;
//0.039;
PlannerState
::
raw_trajectory
.
push_back
(
target_pose_4
);
std
::
vector
<
double
>
x_values
;
std
::
vector
<
double
>
y_values
;
std
::
vector
<
double
>
z_values
;
// check if no trajectory is configured
if
(
!
node_handle
.
getParam
(
"trajectory_pos_x"
,
x_values
)
||
!
node_handle
.
getParam
(
"trajectory_pos_y"
,
y_values
)
||
!
node_handle
.
getParam
(
"trajectory_pos_z"
,
z_values
))
{
return
false
;
}
else
{
for
(
int
i
=
0
;
i
<
x_values
.
size
();
i
++
)
{
geometry_msgs
::
Pose
pose
=
initialPose
;
pose
.
position
.
x
=
x_values
.
at
(
i
);
pose
.
position
.
y
=
y_values
.
at
(
i
);
pose
.
position
.
z
=
z_values
.
at
(
i
);
waypoints
.
push_back
(
pose
);
}
target_pose_1
.
position
.
z
=
group
->
getCurrentPose
().
pose
.
position
.
z
;
target_pose_1
.
position
.
y
=
group
->
getCurrentPose
().
pose
.
position
.
y
;
target_pose_1
.
position
.
x
=
group
->
getCurrentPose
().
pose
.
position
.
x
;
PlannerState
::
raw_trajectory
.
push_back
(
target_pose_1
);
return
true
;
}
}
bool
updateRawTrajectory
(
ros
::
NodeHandle
node_handle
,
moveit
::
planning_interface
::
MoveGroupInterface
*
group
)
{
geometry_msgs
::
Pose
current_pose
=
group
->
getCurrentPose
().
pose
;
PlannerState
::
raw_trajectory
.
clear
();
node_handle
.
setParam
(
"new_trajectory_available"
,
false
);
return
PlannerState
::
traj_util
.
initWaypoints
(
PlannerState
::
raw_trajectory
,
node_handle
,
current_pose
);
}
void
moveRobotToInitialState
(
ros
::
NodeHandle
node_handle
)
{
ROS_INFO
(
"Moving to initial pose of trajectory."
);
moveit
::
planning_interface
::
MoveGroupInterface
group
(
"panda_arm"
);
ros
::
Publisher
display_publisher
=
node_handle
.
advertise
<
moveit_msgs
::
DisplayTrajectory
>
(
"/move_group/display_planned_path"
,
1
,
true
);
group
.
setStartStateToCurrentState
();
std
::
vector
<
geometry_msgs
::
Pose
>
trajectory_to_init_pose
;
// start at beginning of circular movement
tf2
::
Quaternion
orientation
;
// the RPY constructor is deprecated
orientation
.
setRPY
(
0
,
M_PI
,
0
);
geometry_msgs
::
Pose
target_pose_1
;
target_pose_1
.
orientation
=
tf2
::
toMsg
(
orientation
);
target_pose_1
.
position
.
x
=
0
;
target_pose_1
.
position
.
y
=
.6
;
target_pose_1
.
position
.
z
=
.3
;
void
doMotion
(
const
ros
::
NodeHandle
&
node_handle
,
moveit
::
planning_interface
::
MoveGroupInterface
&
group
)
{
trajectory_to_init_pose
.
push_back
(
target_pose_1
)
;
group
.
setPlanningTime
(
10.0
);
bool
newTrajectoryAvailable
=
false
;
node_handle
.
getParam
(
"new_trajectory_available"
,
newTrajectoryAvailable
);
moveit_msgs
::
RobotTrajectory
trajectory
;
double
fraction
=
group
.
computeCartesianPath
(
trajectory_to_init_pose
,
0.01
,
0.0
,
trajectory
);
if
(
newTrajectoryAvailable
)
{
geometry_msgs
::
Pose
current_pose
=
group
.
getCurrentPose
().
pose
;
PlannerState
::
raw_trajectory
.
clear
();
moveit
::
planning_interface
::
MoveGroupInterface
::
Plan
cartesian_plan
;
cartesian_plan
.
trajectory_
=
trajectory
;
group
.
execute
(
cartesian_plan
);
}
if
(
!
loadWaypoints
(
PlannerState
::
raw_trajectory
,
node_handle
,
current_pose
))
{
ROS_ERROR_STREAM
(
"Unable to load new
trajectory
!"
)
;
}
}
void
doMotion
(
const
ros
::
NodeHandle
&
node_handle
,
moveit
::
planning_interface
::
MoveGroupInterface
&
group
)
{
if
(
PlannerState
::
raw_trajectory
.
empty
())
{
return
;
}
for
(
int
i
=
0
;
i
<
PlannerState
::
raw_trajectory
.
size
();
i
++
)
{
bool
newTrajectoryAvailable
=
false
;
node_handle
.
getParam
(
"new_trajectory_available"
,
newTrajectoryAvailable
);
node_handle
.
getParam
(
"loop_trajectory"
,
PlannerState
::
isLooping
);
if
(
newTrajectoryAvailable
)
{
if
(
!
updateRawTrajectory
(
node_handle
,
&
group
))
{
break
;
}
i
=
0
;
}
// wake up if we have trajectory
while
(
PlannerState
::
raw_trajectory
.
empty
())
{
if
(
newTrajectoryAvailable
)
{
updateRawTrajectory
(
node_handle
,
&
group
);
i
=
0
;
break
;
}
}
double
velocity
=
0.0
;
std
::
string
planning_mode
;
double
velocity
;
if
(
!
node_handle
.
getParam
(
"robot_speed_factor"
,
velocity
))
{
velocity
=
PlannerState
::
default_velocity
;
}
std
::
string
planning_mode
;
if
(
!
node_handle
.
getParam
(
"robot_planning_mode"
,
planning_mode
))
{
planning_mode
=
PlannerState
::
default_planning_mode
;
}
...
...
@@ -197,15 +143,18 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
ROS_INFO_STREAM
(
"Planning the next trajectory in "
<<
planning_mode
<<
" with velocity "
<<
velocity
);
moveit
::
planning_interface
::
MoveGroupInterface
::
Plan
plan
;
PlannerState
::
traj_util
.
computePathToPose
(
group
,
plan
,
PlannerState
::
raw_trajectory
.
at
(
i
),
planning_mode
,
0.8
,
0.8
,
velocity
);
group
.
execute
(
plan
);
if
(
PlannerState
::
traj_util
.
computePathToPose
(
group
,
plan
,
PlannerState
::
raw_trajectory
.
at
(
i
),
planning_mode
,
0.8
,
0.8
,
velocity
))
{
group
.
execute
(
plan
);
}
else
{
ROS_ERROR_STREAM
(
"The planner was unable to compute a path to the pose with the given settings."
);
}
// make sure the robot moves in an infinite circle
if
(
i
==
(
PlannerState
::
raw_trajectory
.
size
()
-
1
))
{
if
(
PlannerState
::
isLooping
)
{
i
=
-
1
;
}
else
{
return
;
}
}
}
...
...
@@ -220,27 +169,67 @@ int main(int argc, char **argv) {
spinner
.
start
();
// wait for robot init of robot_state_initializer
ros
::
Duration
(
5
.0
).
sleep
();
ros
::
Duration
(
3
.0
).
sleep
();
ROS_INFO
(
">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< "
);
node_handle
.
setParam
(
"tud_planner_ready"
,
true
);
// Initialize start state of robot and target trajectory.
moveit
::
planning_interface
::
MoveGroupInterface
group
(
"panda_arm"
);
moveit
::
planning_interface
::
PlanningSceneInterface
planning_scene_interface
;
//moveRobotToInitialState(node_handle);
initRawTrajectory
(
node_handle
,
&
group
,
true
,
true
);
node_handle
.
getParam
(
"loop_trajectory"
,
PlannerState
::
isLooping
);
std
::
string
defaultTrajectory
=
"<none>"
;
node_handle
.
getParam
(
"default_trajectory"
,
defaultTrajectory
);
geometry_msgs
::
Pose
base_pose
=
group
.
getCurrentPose
().
pose
;
if
(
loadWaypoints
(
PlannerState
::
raw_trajectory
,
node_handle
,
base_pose
))
{
ROS_INFO_STREAM
(
"Ignoring default waypoints because there is a trajectory defined by parameters"
);
}
else
if
(
defaultTrajectory
==
"circle"
)
{
ROS_INFO_STREAM
(
"loading default trajectory 'circle'"
);
geometry_msgs
::
Point
origin
;
origin
.
z
=
.3
;
loadCircularTrajectory
(
PlannerState
::
raw_trajectory
,
0.6
,
origin
,
20
);
}
else
if
(
defaultTrajectory
==
"square"
)
{
ROS_INFO_STREAM
(
"loading default trajectory 'square'"
);
// choose a default trajectory
base_pose
.
position
.
x
=
0.4
;
base_pose
.
position
.
y
=
0.4
;
base_pose
.
position
.
z
=
0.3
;
geometry_msgs
::
Pose
target_pose_1
=
base_pose
;
geometry_msgs
::
Pose
target_pose_2
=
base_pose
;
geometry_msgs
::
Pose
target_pose_3
=
base_pose
;
geometry_msgs
::
Pose
target_pose_4
=
base_pose
;
PlannerState
::
raw_trajectory
.
push_back
(
target_pose_1
);
target_pose_2
.
position
.
y
*=
-
1
;
PlannerState
::
raw_trajectory
.
push_back
(
target_pose_2
);
target_pose_3
.
position
.
x
*=
-
1
;
target_pose_3
.
position
.
y
*=
-
1
;
PlannerState
::
raw_trajectory
.
push_back
(
target_pose_3
);
target_pose_4
.
position
.
x
*=
-
1
;
PlannerState
::
raw_trajectory
.
push_back
(
target_pose_4
);
}
else
{
ROS_WARN_STREAM
(
"not loading a default trajectory! provided trajectory: "
+
defaultTrajectory
);
}
// add the ground plate
std
::
vector
<
moveit_msgs
::
CollisionObject
>
collision_objects
;
constructPlate
(
collision_objects
,
5.0
,
5.0
);
planning_scene_interface
.
applyCollisionObjects
(
collision_objects
);
// execute the trajectory which consists of single waypoints to allow distinct planning
doMotion
(
node_handle
,
group
);
while
(
ros
::
ok
())
{
// execute a trajectory
doMotion
(
node_handle
,
group
);
ros
::
spinOnce
();
}
ros
::
shutdown
();
return
0
;
}
This diff is collapsed.
Click to expand it.
src/util/TrajectoryUtil.cpp
+
3
−
26
View file @
a7ebe96d
...
...
@@ -61,6 +61,7 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
}
else
if
(
pathType
==
FLUID_PATH
)
{
if
(
group
.
plan
(
plan
)
!=
moveit
::
planning_interface
::
MoveItErrorCode
::
SUCCESS
)
{
ROS_ERROR
(
"Computation of fluid path has failed."
);
ROS_ERROR_STREAM
(
"Path was motion to:
\n
"
<<
targetPose
);
}
}
else
{
ROS_ERROR
(
"Invalid type of path provided."
);
...
...
@@ -68,35 +69,11 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
}
manipulateVelocity
(
plan
,
motionSpeedFactor
);
}
bool
TrajectoryUtil
::
initWaypoints
(
std
::
vector
<
geometry_msgs
::
Pose
>
&
waypoints
,
ros
::
NodeHandle
&
node_handle
,
geometry_msgs
::
Pose
&
initialPose
)
{
std
::
vector
<
double
>
x_values
;
std
::
vector
<
double
>
y_values
;
std
::
vector
<
double
>
z_values
;
// check if no trajectory is configured
if
(
!
node_handle
.
getParam
(
"trajectory_pos_x"
,
x_values
)
||
!
node_handle
.
getParam
(
"trajectory_pos_y"
,
y_values
)
||
!
node_handle
.
getParam
(
"trajectory_pos_z"
,
z_values
))
{
return
false
;
}
else
{
for
(
int
i
=
0
;
i
<
x_values
.
size
();
i
++
)
{
geometry_msgs
::
Pose
pose
=
initialPose
;
pose
.
position
.
x
=
x_values
.
at
(
i
);
pose
.
position
.
y
=
y_values
.
at
(
i
);
pose
.
position
.
z
=
z_values
.
at
(
i
);
waypoints
.
push_back
(
pose
);
}
node_handle
.
setParam
(
"new_trajectory_available"
,
false
);
return
true
;
}
return
true
;
}
void
TrajectoryUtil
::
manipulateVelocity
(
moveit
::
planning_interface
::
MoveGroupInterface
::
Plan
&
plan
,
double
velocity
)
{
ROS_INFO_STREAM
(
"slowing down plan with "
<<
plan
.
trajectory_
.
joint_trajectory
.
points
.
size
()
<<
"joints"
);
ROS_INFO_STREAM
(
"slowing down plan with "
<<
plan
.
trajectory_
.
joint_trajectory
.
points
.
size
()
<<
"
joints"
);
for
(
auto
&
point
:
plan
.
trajectory_
.
joint_trajectory
.
points
)
{
point
.
time_from_start
*=
1
/
velocity
;
for
(
double
&
v
:
point
.
velocities
)
{
...
...
This diff is collapsed.
Click to expand it.
src/util/TrajectoryUtil.h
+
1
−
4
View file @
a7ebe96d
...
...
@@ -27,12 +27,9 @@ public:
const
std
::
string
&
pathType
,
double
maxVelocityFactor
,
double
maxAccelerationFactor
,
double
motionSpeedFactor
);
bool
initWaypoints
(
std
::
vector
<
geometry_msgs
::
Pose
>
&
waypoints
,
ros
::
NodeHandle
&
node_handle
,
geometry_msgs
::
Pose
&
initialPose
);
private:
void
manipulateVelocity
(
moveit
::
planning_interface
::
MoveGroupInterface
::
Plan
&
plan
,
double
velocity
);
static
void
manipulateVelocity
(
moveit
::
planning_interface
::
MoveGroupInterface
::
Plan
&
plan
,
double
velocity
);
};
#endif //PANDA_SIMULATION_TRAJECTORYUTIL_H
\ No newline at end of file
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment