thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Compare commits

base: thomas:a0ee92d53a0d62840aef33587854f79a9c901c9a
Branches Tags
thomas:master
thomas:kinda_works
thomas:brokenaf
..
compare: thomas:293f603a8b81a387d6e5694cadc52db7f39c364e
Branches Tags
thomas:kinda_works
thomas:brokenaf
thomas:master

No commits in common. "a0ee92d53a0d62840aef33587854f79a9c901c9a" and "293f603a8b81a387d6e5694cadc52db7f39c364e" have entirely different histories.
a0ee92d53a ... 293f603a8b

4 changed files with 37 additions and 68 deletions
Show stats Expand all files Collapse all files

9
src/ar3_config/config/ar3_controllers.yaml
View file

@ -1,16 +1,7 @@
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
trajectory_execution:
allowed_execution_duration_scaling: 1.2
allowed_goal_duration_margin: 0.5
allowed_start_tolerance: 0.01
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
controller_names:
- joint_trajectory_controller
joint_trajectory_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory

52
src/ar3_config/launch/dev.launch.py
View file

@ -34,7 +34,7 @@ def generate_launch_description():
executable='rviz2',
output='log',
arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
parameters=[robot_description, robot_description_semantic, robot_description_kinematics, get_yaml(join(config_root, 'ompl_planning.yaml'))]
parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
),
# Attatch robot to map
@ -50,21 +50,21 @@ def generate_launch_description():
package='move_group_interface',
executable='move_group_interface_node',
output='screen',
parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
#get_yaml(join(config_root, 'ompl_planning.yaml')),
#get_yaml(join(config_root, 'trajectory.yaml')),
#get_yaml(join(config_root, 'planning_scene.yaml')),
#get_yaml(join(config_root, "ar3_controllers.yaml")),
#{ "default_planning_pipeline": "ompl" },
#{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
parameters=[robot_description, robot_description_semantic, robot_description_kinematics,
get_yaml(join(config_root, 'ompl_planning.yaml')),
get_yaml(join(config_root, 'trajectory.yaml')),
get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml")),
{ "default_planning_pipeline": "ompl" },
{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
),
# Our joystick node
#Node(
# package='joystickmove',
# executable='move',
# output='screen',
#),
Node(
package='joystickmove',
executable='move',
output='screen',
),
# Create joystick controller and ComposableNodes for servo_server
ComposableNodeContainer(
@ -73,18 +73,18 @@ def generate_launch_description():
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[
#ComposableNode(
# package="moveit_servo",
# plugin="moveit_servo::ServoServer",
# name="servo_server",
# parameters=[
# {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
# robot_description,
# robot_description_semantic,
# robot_description_kinematics,
# ],
# extra_arguments=[{"use_intra_process_comms": True}],
#),
ComposableNode(
package="moveit_servo",
plugin="moveit_servo::ServoServer",
name="servo_server",
parameters=[
{'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
robot_description,
robot_description_semantic,
robot_description_kinematics,
],
extra_arguments=[{"use_intra_process_comms": True}],
),
ComposableNode(
package="joy",
plugin="joy::Joy",
@ -134,6 +134,8 @@ def generate_launch_description():
get_yaml(join(config_root, 'trajectory.yaml')),
get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml")),
{ "default_planning_pipeline": "move_group" },
{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
],
),

22
src/cobot_platform/cobot_platform/move.py
View file

@ -9,14 +9,13 @@ from math import pi
from serial import Serial
J1_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 10 * 4
J2_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
J3_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
J4_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (13 + 212 / 289) * 2.8
J5_COUNTS_PER_RAD = 1 / (2 * pi) * 800 * 10
J6_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (19 + 38 / 187)
class CobotMove(Node):
J1_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 10 * 4
J2_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
J3_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
J4_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (13 + 212 / 289) * 2.8
J5_COUNTS_PER_RAD = 1 / (2 * pi) * 800 * 10
J6_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (19 + 38 / 187)
def __init__(self):
super().__init__('cobot_move')
@ -31,7 +30,7 @@ class CobotMove(Node):
self._cur = [0.0] * 6
# create a timer to update all the joint angles at once
self._timer = self.create_timer(1 / 40, self._dosomething)
self._timer = self.create_timer(1 / 20, self._dosomething)
# Set joint angles independantly
base_topic = 'move/'
@ -92,13 +91,6 @@ class CobotMove(Node):
e = float(blocks[4][1:]) / J5_COUNTS_PER_RAD
f = float(blocks[5][1:]) / J6_COUNTS_PER_RAD
a = float(self._cur[0] / J1_COUNTS_PER_RAD)
b = float(self._cur[1] / J2_COUNTS_PER_RAD)
c = float(self._cur[2] / J3_COUNTS_PER_RAD)
d = float(self._cur[3] / J4_COUNTS_PER_RAD)
e = float(self._cur[4] / J5_COUNTS_PER_RAD)
f = float(self._cur[5] / J6_COUNTS_PER_RAD)
# Publish those to the update/jX subscribers
self.s_j1.publish(Float32(data=a))
self.s_j2.publish(Float32(data=b))

22
src/move_group_interface/src/move_group_interface_node.cpp
View file

@ -15,9 +15,6 @@
#include <moveit/common_planning_interface_objects/common_objects.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/rdf_loader/rdf_loader.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "geometry_msgs/msg/pose_stamped.hpp"
@ -26,24 +23,17 @@ const std::string PLANNING_GROUP = "ar3_arm";
class ScanNPlan : public rclcpp::Node {
public:
ScanNPlan() : Node("scan_n_plan") {
callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions options;
options.callback_group = callback_group_;
// Create /move_pose subscriber
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_pose",
rclcpp::QoS(1),
std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1),
options
std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1)
);
relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_relative_pose",
rclcpp::QoS(1),
std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1),
options
std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1)
);
}
@ -56,7 +46,6 @@ private:
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
RCLCPP_INFO(this->get_logger(), "GOT MSG");
move_group_->setStartStateToCurrentState();
move_group_->setPoseTarget(p->pose);
moveit::planning_interface::MoveGroupInterface::Plan plan;
@ -89,8 +78,6 @@ private:
move_group_->move();
}
private:
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;
@ -104,11 +91,8 @@ int main(int argc, char* argv[]) {
auto node = std::make_shared<ScanNPlan>();
RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
rclcpp::executors::MultiThreadedExecutor executor;
node->init();
executor.add_node(node);
executor.spin();
// rclcpp::spin(node);
rclcpp::spin(node);
RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
rclcpp::shutdown();