src/ar3_config/config/ar3_controllers.yaml

@@ -1,15 +1,14 @@
-moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+controller_names:
-moveit_simple_controller_manager:
+  - joint_trajectory_controller
-  controller_names:
+
-    - joint_trajectory_controller
+joint_trajectory_controller:
-  joint_trajectory_controller:
+  action_ns: follow_joint_trajectory
-    action_ns: follow_joint_trajectory
+  type: FollowJointTrajectory
-    type: FollowJointTrajectory
+  default: true
-    default: true
+  joints:
-    joints:
+    - joint_1
-      - joint_1
+    - joint_2
-      - joint_2
+    - joint_3
-      - joint_3
+    - joint_4
-      - joint_4
+    - joint_5
-      - joint_5
+    - joint_6
-      - joint_6

src/ar3_config/config/ompl_planning.yaml

@@ -1,156 +1,149 @@
-move_group:
+planner_configs:
-  planning_plugin: ompl_interface/OMPLPlanner
+  SBLkConfigDefault:
-  request_adapters:
+    type: geometric::SBL
-    default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  start_state_max_bounds_error: 0.1
+  ESTkConfigDefault:
+    type: geometric::EST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LBKPIECEkConfigDefault:
+    type: geometric::LBKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  BKPIECEkConfigDefault:
+    type: geometric::BKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  KPIECEkConfigDefault:
+    type: geometric::KPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  RRTkConfigDefault:
+    type: geometric::RRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+  RRTConnectkConfigDefault:
+    type: geometric::RRTConnect
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  RRTstarkConfigDefault:
+    type: geometric::RRTstar
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
+  TRRTkConfigDefault:
+    type: geometric::TRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    max_states_failed: 10  # when to start increasing temp. default: 10
+    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
+    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
+    init_temperature: 10e-6  # initial temperature. default: 10e-6
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
+  PRMkConfigDefault:
+    type: geometric::PRM
+    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
+  PRMstarkConfigDefault:
+    type: geometric::PRMstar
+  FMTkConfigDefault:
+    type: geometric::FMT
+    num_samples: 1000  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
+    nearest_k: 1  # use Knearest strategy. default: 1
+    cache_cc: 1  # use collision checking cache. default: 1
+    heuristics: 0  # activate cost to go heuristics. default: 0
+    extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+  BFMTkConfigDefault:
+    type: geometric::BFMT
+    num_samples: 1000  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
+    nearest_k: 1  # use the Knearest strategy. default: 1
+    balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
+    optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
+    heuristics: 1  # activates cost to go heuristics. default: 1
+    cache_cc: 1  # use the collision checking cache. default: 1
+    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+  PDSTkConfigDefault:
+    type: geometric::PDST
+  STRIDEkConfigDefault:
+    type: geometric::STRIDE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
+    degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
+    max_degree: 18  # max degree of a node in the GNAT. default: 12
+    min_degree: 12  # min degree of a node in the GNAT. default: 12
+    max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
+    estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
+    min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
+  BiTRRTkConfigDefault:
+    type: geometric::BiTRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
+    init_temperature: 100  # initial temperature. default: 100
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
+  LBTRRTkConfigDefault:
+    type: geometric::LBTRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    epsilon: 0.4  # optimality approximation factor. default: 0.4
+  BiESTkConfigDefault:
+    type: geometric::BiEST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ProjESTkConfigDefault:
+    type: geometric::ProjEST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LazyPRMkConfigDefault:
+    type: geometric::LazyPRM
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  LazyPRMstarkConfigDefault:
+    type: geometric::LazyPRMstar
+  SPARSkConfigDefault:
+    type: geometric::SPARS
+    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
+    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+    max_failures: 1000  # maximum consecutive failure limit. default: 1000
+  SPARStwokConfigDefault:
+    type: geometric::SPARStwo
+    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
+    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+    max_failures: 5000  # maximum consecutive failure limit. default: 5000
 
+ar3_arm:
+  default_planner_config: RRTConnectkConfigDefault
   planner_configs:
-    SBLkConfigDefault:
+    - SBLkConfigDefault
-      type: geometric::SBL
+    - ESTkConfigDefault
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    - LBKPIECEkConfigDefault
-    ESTkConfigDefault:
+    - BKPIECEkConfigDefault
-      type: geometric::EST
+    - KPIECEkConfigDefault
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+    - RRTkConfigDefault
-      goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    - RRTConnectkConfigDefault
-    LBKPIECEkConfigDefault:
+    - RRTstarkConfigDefault
-      type: geometric::LBKPIECE
+    - TRRTkConfigDefault
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    - PRMkConfigDefault
-      border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    - PRMstarkConfigDefault
-      min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+    - FMTkConfigDefault
-    BKPIECEkConfigDefault:
+    - BFMTkConfigDefault
-      type: geometric::BKPIECE
+    - PDSTkConfigDefault
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    - STRIDEkConfigDefault
-      border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    - BiTRRTkConfigDefault
-      failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    - LBTRRTkConfigDefault
-      min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+    - BiESTkConfigDefault
-    KPIECEkConfigDefault:
+    - ProjESTkConfigDefault
-      type: geometric::KPIECE
+    - LazyPRMkConfigDefault
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    - LazyPRMstarkConfigDefault
-      goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    - SPARSkConfigDefault
-      border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+    - SPARStwokConfigDefault
-      failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
-      min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
-    RRTkConfigDefault:
-      type: geometric::RRT
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-      goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
-    RRTConnectkConfigDefault:
-      type: geometric::RRTConnect
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    RRTstarkConfigDefault:
-      type: geometric::RRTstar
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-      goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
-      delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
-    TRRTkConfigDefault:
-      type: geometric::TRRT
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-      goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
-      max_states_failed: 10  # when to start increasing temp. default: 10
-      temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
-      min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
-      init_temperature: 10e-6  # initial temperature. default: 10e-6
-      frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
-      frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
-      k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
-    PRMkConfigDefault:
-      type: geometric::PRM
-      max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
-    PRMstarkConfigDefault:
-      type: geometric::PRMstar
-    FMTkConfigDefault:
-      type: geometric::FMT
-      num_samples: 1000  # number of states that the planner should sample. default: 1000
-      radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
-      nearest_k: 1  # use Knearest strategy. default: 1
-      cache_cc: 1  # use collision checking cache. default: 1
-      heuristics: 0  # activate cost to go heuristics. default: 0
-      extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
-    BFMTkConfigDefault:
-      type: geometric::BFMT
-      num_samples: 1000  # number of states that the planner should sample. default: 1000
-      radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
-      nearest_k: 1  # use the Knearest strategy. default: 1
-      balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
-      optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
-      heuristics: 1  # activates cost to go heuristics. default: 1
-      cache_cc: 1  # use the collision checking cache. default: 1
-      extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
-    PDSTkConfigDefault:
-      type: geometric::PDST
-    STRIDEkConfigDefault:
-      type: geometric::STRIDE
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-      goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
-      use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
-      degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
-      max_degree: 18  # max degree of a node in the GNAT. default: 12
-      min_degree: 12  # min degree of a node in the GNAT. default: 12
-      max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
-      estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
-      min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
-    BiTRRTkConfigDefault:
-      type: geometric::BiTRRT
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-      temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
-      init_temperature: 100  # initial temperature. default: 100
-      frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
-      frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
-      cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
-    LBTRRTkConfigDefault:
-      type: geometric::LBTRRT
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-      goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
-      epsilon: 0.4  # optimality approximation factor. default: 0.4
-    BiESTkConfigDefault:
-      type: geometric::BiEST
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    ProjESTkConfigDefault:
-      type: geometric::ProjEST
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-      goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
-    LazyPRMkConfigDefault:
-      type: geometric::LazyPRM
-      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    LazyPRMstarkConfigDefault:
-      type: geometric::LazyPRMstar
-    SPARSkConfigDefault:
-      type: geometric::SPARS
-      stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
-      sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
-      dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
-      max_failures: 1000  # maximum consecutive failure limit. default: 1000
-    SPARStwokConfigDefault:
-      type: geometric::SPARStwo
-      stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
-      sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
-      dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
-      max_failures: 5000  # maximum consecutive failure limit. default: 5000
-
-  ar3_arm:
-    default_planner_config: RRTConnectkConfigDefault
-    planner_configs:
-      - SBLkConfigDefault
-      - ESTkConfigDefault
-      - LBKPIECEkConfigDefault
-      - BKPIECEkConfigDefault
-      - KPIECEkConfigDefault
-      - RRTkConfigDefault
-      - RRTConnectkConfigDefault
-      - RRTstarkConfigDefault
-      - TRRTkConfigDefault
-      - PRMkConfigDefault
-      - PRMstarkConfigDefault
-      - FMTkConfigDefault
-      - BFMTkConfigDefault
-      - PDSTkConfigDefault
-      - STRIDEkConfigDefault
-      - BiTRRTkConfigDefault
-      - LBTRRTkConfigDefault
-      - BiESTkConfigDefault
-      - ProjESTkConfigDefault
-      - LazyPRMkConfigDefault
-      - LazyPRMstarkConfigDefault
-      - SPARSkConfigDefault
-      - SPARStwokConfigDefault
-

src/ar3_config/config/planning_scene.yaml

@@ -1,4 +0,0 @@
-publish_planning_scene: true
-publish_geometry_updates: true
-publish_state_updates: true
-publish_transforms_updates: true

src/ar3_config/config/trajectory.yaml

@@ -1,4 +0,0 @@
-moveit_manage_controllers: true
-trajectory_execution.allowed_execution_duration_scaling: 1.2
-trajectory_execution.allowed_goal_duration_margin: 0.5
-trajectory_execution.allowed_start_tolerance: 0.1

src/ar3_config/launch/dev.launch.py

@@ -1,7 +1,8 @@
 from os.path import join
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
 from launch.actions import ExecuteProcess
 import yaml
 
@@ -30,7 +31,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]
+            parameters=[robot_description, robot_description_semantic, robot_description_kinematics],
         ),
 
         # Attatch robot to map
@@ -38,7 +39,7 @@ def generate_launch_description():
             package='tf2_ros',
             executable='static_transform_publisher',
             output='screen',
-            arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
+            arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link'],
         ),
 
         Node(
@@ -73,30 +74,25 @@ def generate_launch_description():
                 )
             ],
             output="screen",
-        )
+        ),
 
         # Controller
         Node(
             package='controller_manager',
             executable='ros2_control_node',
             output='screen',
-            parameters=[robot_description, join(config_root, 'controller_manager.yaml')]
+            parameters=[robot_description, join(config_root, 'controller_manager.yaml')],
         ),
 
         # Start controllers
         ExecuteProcess(
             cmd=['ros2 run controller_manager spawner.py joint_trajectory_controller'],
             shell=True,
-            output='screen'),
+            output='screen',
+        ),
         ExecuteProcess(
             cmd=['ros2 run controller_manager spawner.py joint_state_controller'],
             shell=True,
-            output='screen'),
-
-        # Run platform
-        Node(
-            package='cobot_platform',
-            executable='move',
             output='screen',
         ),
 
@@ -109,11 +105,9 @@ def generate_launch_description():
                 robot_description,
                 robot_description_semantic,
                 robot_description_kinematics,
-                get_yaml(join(config_root, 'ompl_planning.yaml')),
+                {'move_group': join(config_root, 'ompl_planning.yaml')},
-                get_yaml(join(config_root, 'trajectory.yaml')),
+                {'moveit_simple_controller_manager': join(config_root, "ar3_controllers.yaml")},
-                get_yaml(join(config_root, 'planning_scene.yaml')),
+            ]
-                get_yaml(join(config_root, "ar3_controllers.yaml")),
-            ],
         ),
 
         # Robot state publisher
@@ -121,5 +115,14 @@ def generate_launch_description():
             package='robot_state_publisher',
             executable='robot_state_publisher',
             output='screen',
-            parameters=[robot_description]),
+            parameters=[robot_description],
+        ),
+
+        # Interface with arduino
+        Node(
+            package='cobot_platform',
+            executable='move',
+            output='screen',
+        ),
     ])
+

src/cobot_platform/cobot_platform/move.py

@@ -46,11 +46,11 @@ class CobotMove(Node):
         print(self._serial.readline())
 
     def _move_j1_cb(self, msg):
-        self._cur[0] = int(msg.data / pi / 2. * 8000)
+        self._cur[0] = int(msg.data / pi / 2. * 2000)
         self._move_servo()
 
     def _move_j2_cb(self, msg):
-        self._cur[1] = int(msg.data / pi / 2. * 10000)
+        self._cur[1] = int(msg.data / pi / 2. * 3840.64)
         self._move_servo()
 
     def _move_j3_cb(self, msg):
@@ -105,4 +105,3 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()
-