diff --git a/movo_7dof_moveit_config/config/controllers_left_only.yaml b/movo_7dof_moveit_config/config/controllers_left_only.yaml
new file mode 100644
index 00000000..65d74669
--- /dev/null
+++ b/movo_7dof_moveit_config/config/controllers_left_only.yaml
@@ -0,0 +1,36 @@
+controller_list:
+ - name: "movo/left_arm_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - left_shoulder_pan_joint
+ - left_shoulder_lift_joint
+ - left_arm_half_joint
+ - left_elbow_joint
+ - left_wrist_spherical_1_joint
+ - left_wrist_spherical_2_joint
+ - left_wrist_3_joint
+
+ - name: "movo/torso_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - linear_joint
+
+ - name: "movo/head_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - pan_joint
+ - tilt_joint
+
+ - name: "movo/left_gripper_controller"
+ action_ns: gripper_cmd
+ type: GripperCommand
+ default: true
+ joints:
+ - left_gripper_finger1_joint
+
diff --git a/movo_7dof_moveit_config/config/controllers_right_only.yaml b/movo_7dof_moveit_config/config/controllers_right_only.yaml
new file mode 100644
index 00000000..85b5b593
--- /dev/null
+++ b/movo_7dof_moveit_config/config/controllers_right_only.yaml
@@ -0,0 +1,36 @@
+controller_list:
+ - name: "movo/right_arm_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - right_shoulder_pan_joint
+ - right_shoulder_lift_joint
+ - right_arm_half_joint
+ - right_elbow_joint
+ - right_wrist_spherical_1_joint
+ - right_wrist_spherical_2_joint
+ - right_wrist_3_joint
+
+ - name: "movo/torso_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - linear_joint
+
+ - name: "movo/head_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - pan_joint
+ - tilt_joint
+
+ - name: "movo/right_gripper_controller"
+ action_ns: gripper_cmd
+ type: GripperCommand
+ default: true
+ joints:
+ - right_gripper_finger1_joint
+
diff --git a/movo_7dof_moveit_config/config/fake_controllers_left_only.yaml b/movo_7dof_moveit_config/config/fake_controllers_left_only.yaml
new file mode 100644
index 00000000..6f3a396f
--- /dev/null
+++ b/movo_7dof_moveit_config/config/fake_controllers_left_only.yaml
@@ -0,0 +1,42 @@
+controller_list:
+ - name: fake_left_arm_controller
+ joints:
+ - left_shoulder_pan_joint
+ - left_shoulder_lift_joint
+ - left_arm_half_joint
+ - left_elbow_joint
+ - left_wrist_spherical_1_joint
+ - left_wrist_spherical_2_joint
+ - left_wrist_3_joint
+ - name: fake_torso_controller
+ joints:
+ - linear_joint
+ - name: fake_left_gripper_controller
+ joints:
+ - left_gripper_finger1_joint
+ - name: fake_head_controller
+ joints:
+ - pan_joint
+ - tilt_joint
+ - name: fake_left_side_controller
+ joints:
+ - linear_joint
+ - left_shoulder_pan_joint
+ - left_shoulder_lift_joint
+ - left_arm_half_joint
+ - left_elbow_joint
+ - left_wrist_spherical_1_joint
+ - left_wrist_spherical_2_joint
+ - left_wrist_3_joint
+ - name: fake_upper_body_controller
+ joints:
+ - linear_joint
+ - left_shoulder_pan_joint
+ - left_shoulder_lift_joint
+ - left_arm_half_joint
+ - left_elbow_joint
+ - left_wrist_spherical_1_joint
+ - left_wrist_spherical_2_joint
+ - left_wrist_3_joint
+ - pan_joint
+ - tilt_joint
diff --git a/movo_7dof_moveit_config/config/fake_controllers_right_only.yaml b/movo_7dof_moveit_config/config/fake_controllers_right_only.yaml
new file mode 100644
index 00000000..8611249d
--- /dev/null
+++ b/movo_7dof_moveit_config/config/fake_controllers_right_only.yaml
@@ -0,0 +1,42 @@
+controller_list:
+ - name: fake_right_arm_controller
+ joints:
+ - right_shoulder_pan_joint
+ - right_shoulder_lift_joint
+ - right_arm_half_joint
+ - right_elbow_joint
+ - right_wrist_spherical_1_joint
+ - right_wrist_spherical_2_joint
+ - right_wrist_3_joint
+ - name: fake_torso_controller
+ joints:
+ - linear_joint
+ - name: fake_right_gripper_controller
+ joints:
+ - right_gripper_finger1_joint
+ - name: fake_head_controller
+ joints:
+ - pan_joint
+ - tilt_joint
+ - name: fake_right_side_controller
+ joints:
+ - linear_joint
+ - right_shoulder_pan_joint
+ - right_shoulder_lift_joint
+ - right_arm_half_joint
+ - right_elbow_joint
+ - right_wrist_spherical_1_joint
+ - right_wrist_spherical_2_joint
+ - right_wrist_3_joint
+ - name: fake_upper_body_controller
+ joints:
+ - linear_joint
+ - pan_joint
+ - tilt_joint
+ - right_shoulder_pan_joint
+ - right_shoulder_lift_joint
+ - right_arm_half_joint
+ - right_elbow_joint
+ - right_wrist_spherical_1_joint
+ - right_wrist_spherical_2_joint
+ - right_wrist_3_joint
diff --git a/movo_7dof_moveit_config/config/kinematics_left_only.yaml b/movo_7dof_moveit_config/config/kinematics_left_only.yaml
new file mode 100644
index 00000000..c3e5346e
--- /dev/null
+++ b/movo_7dof_moveit_config/config/kinematics_left_only.yaml
@@ -0,0 +1,12 @@
+left_arm:
+ kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
+ kinematics_solver_attempts: 5
+ solve_type: Distance
+left_side:
+ kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
+ kinematics_solver_attempts: 5
+ solve_type: Distance
diff --git a/movo_7dof_moveit_config/config/kinematics_right_only.yaml b/movo_7dof_moveit_config/config/kinematics_right_only.yaml
new file mode 100644
index 00000000..a1489ed3
--- /dev/null
+++ b/movo_7dof_moveit_config/config/kinematics_right_only.yaml
@@ -0,0 +1,12 @@
+right_arm:
+ kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
+ kinematics_solver_attempts: 5
+ solve_type: Distance
+right_side:
+ kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
+ kinematics_solver_attempts: 5
+ solve_type: Distance
diff --git a/movo_7dof_moveit_config/config/movo_kg2_left_only.srdf b/movo_7dof_moveit_config/config/movo_kg2_left_only.srdf
new file mode 100644
index 00000000..785aee88
--- /dev/null
+++ b/movo_7dof_moveit_config/config/movo_kg2_left_only.srdf
@@ -0,0 +1,242 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_7dof_moveit_config/config/movo_kg2_right_only.srdf b/movo_7dof_moveit_config/config/movo_kg2_right_only.srdf
new file mode 100644
index 00000000..2e29932f
--- /dev/null
+++ b/movo_7dof_moveit_config/config/movo_kg2_right_only.srdf
@@ -0,0 +1,248 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_7dof_moveit_config/config/movo_kg3_left_only.srdf b/movo_7dof_moveit_config/config/movo_kg3_left_only.srdf
new file mode 100644
index 00000000..886f1ec3
--- /dev/null
+++ b/movo_7dof_moveit_config/config/movo_kg3_left_only.srdf
@@ -0,0 +1,269 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_7dof_moveit_config/config/movo_kg3_right_only.srdf b/movo_7dof_moveit_config/config/movo_kg3_right_only.srdf
new file mode 100644
index 00000000..c43eaa45
--- /dev/null
+++ b/movo_7dof_moveit_config/config/movo_kg3_right_only.srdf
@@ -0,0 +1,264 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_7dof_moveit_config/config/movo_r85_left_only.srdf b/movo_7dof_moveit_config/config/movo_r85_left_only.srdf
new file mode 100644
index 00000000..48fcf6ab
--- /dev/null
+++ b/movo_7dof_moveit_config/config/movo_r85_left_only.srdf
@@ -0,0 +1,298 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_7dof_moveit_config/config/movo_r85_right_only.srdf b/movo_7dof_moveit_config/config/movo_r85_right_only.srdf
new file mode 100644
index 00000000..36e9519a
--- /dev/null
+++ b/movo_7dof_moveit_config/config/movo_r85_right_only.srdf
@@ -0,0 +1,538 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_7dof_moveit_config/config/ompl_planning_left_only.yaml b/movo_7dof_moveit_config/config/ompl_planning_left_only.yaml
new file mode 100644
index 00000000..f39bd2fd
--- /dev/null
+++ b/movo_7dof_moveit_config/config/ompl_planning_left_only.yaml
@@ -0,0 +1,130 @@
+planner_configs:
+ SBLkConfigDefault:
+ type: geometric::SBL
+ range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+ 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
+left_arm:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(left_shoulder_pan_joint,left_shoulder_lift_joint)
+ longest_valid_segment_fraction: 0.05
+torso:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+head:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+left_side:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(linear_joint,left_shoulder_pan_joint)
+ longest_valid_segment_fraction: 0.05
+upper_body:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(left_shoulder_pan_joint)
+ longest_valid_segment_fraction: 0.05
diff --git a/movo_7dof_moveit_config/config/ompl_planning_right_only.yaml b/movo_7dof_moveit_config/config/ompl_planning_right_only.yaml
new file mode 100644
index 00000000..51632b94
--- /dev/null
+++ b/movo_7dof_moveit_config/config/ompl_planning_right_only.yaml
@@ -0,0 +1,130 @@
+planner_configs:
+ SBLkConfigDefault:
+ type: geometric::SBL
+ range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+ 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
+right_arm:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(right_shoulder_pan_joint,right_shoulder_lift_joint)
+ longest_valid_segment_fraction: 0.05
+torso:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+head:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+right_side:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(linear_joint,right_shoulder_pan_joint)
+ longest_valid_segment_fraction: 0.02
+upper_body:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(right_shoulder_pan_joint)
+ longest_valid_segment_fraction: 0.05
diff --git a/movo_7dof_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/movo_7dof_moveit_config/launch/fake_moveit_controller_manager.launch.xml
index 20757d0a..dfc5adce 100644
--- a/movo_7dof_moveit_config/launch/fake_moveit_controller_manager.launch.xml
+++ b/movo_7dof_moveit_config/launch/fake_moveit_controller_manager.launch.xml
@@ -4,6 +4,7 @@
-
-
+
+
+
diff --git a/movo_7dof_moveit_config/launch/movo_moveit_controller_manager.launch.xml b/movo_7dof_moveit_config/launch/movo_moveit_controller_manager.launch.xml
index 17a2b4e3..f6158bfb 100644
--- a/movo_7dof_moveit_config/launch/movo_moveit_controller_manager.launch.xml
+++ b/movo_7dof_moveit_config/launch/movo_moveit_controller_manager.launch.xml
@@ -4,7 +4,9 @@
-
+
+
+
+
+
+
diff --git a/movo_7dof_moveit_config/launch/planning_context.launch b/movo_7dof_moveit_config/launch/planning_context.launch
index bcd75940..d468efc0 100644
--- a/movo_7dof_moveit_config/launch/planning_context.launch
+++ b/movo_7dof_moveit_config/launch/planning_context.launch
@@ -6,23 +6,41 @@
-
+
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
+
+
+
+
+
diff --git a/movo_common/movo_config/movo_config.bash b/movo_common/movo_config/movo_config.bash
index d1385d43..c3d9c32b 100644
--- a/movo_common/movo_config/movo_config.bash
+++ b/movo_common/movo_config/movo_config.bash
@@ -1,3 +1,5 @@
+#!/bin/bash
+
# This configures the environment variables for a MoVo simulation
# This is necessary to run before starting the simulation
#
@@ -6,6 +8,8 @@
# to make sure it gets gracefully shutdown before the system power cuts out.
#export MOVO_POWERS_PC_ONBOARD=true
+unset "${!MOVO_HAS_@}"
+
#Default Movo network
export MOVO_IP_ADDRESS=10.66.171.5
@@ -62,13 +66,60 @@ export LASER2_MIN_ANGLE=-2.0
export LASER2_PREFIX="rear"
#Kinova arm configurations (the right arm should be the default if there is only one)
-export MOVO_HAS_KINOVA_ARM=true
-export MOVO_HAS_TWO_KINOVA_ARMS=true
+#export MOVO_HAS_KINOVA_ARM=true
+#export MOVO_HAS_TWO_KINOVA_ARMS=true
+
+export MOVO_HAS_RIGHT_ARM_6DOF=false
+export MOVO_HAS_RIGHT_ARM_7DOF=true
+export MOVO_HAS_LEFT_ARM_6DOF=false
+export MOVO_HAS_LEFT_ARM_7DOF=true
+
+
+# automatically set variables. do not touch
+if $MOVO_HAS_RIGHT_ARM_7DOF && $MOVO_HAS_LEFT_ARM_7DOF; then
+ export MOVO_HAS_TWO_7DOF_ARMS=true
+else
+ export MOVO_HAS_TWO_7DOF_ARMS=false
+fi
+if $MOVO_HAS_RIGHT_ARM_6DOF && $MOVO_HAS_LEFT_ARM_6DOF; then
+ export MOVO_HAS_TWO_6DOF_ARMS=true
+else
+ export MOVO_HAS_TWO_6DOF_ARMS=false
+fi
+if [ $MOVO_HAS_RIGHT_ARM_7DOF = false ] && [ $MOVO_HAS_RIGHT_ARM_6DOF = false ]; then
+ export MOVO_HAS_NO_RIGHT_ARM=true
+else
+ export MOVO_HAS_NO_RIGHT_ARM=false
+fi
+if [ $MOVO_HAS_LEFT_ARM_7DOF = false ] && [ $MOVO_HAS_LEFT_ARM_6DOF = false ]; then
+ export MOVO_HAS_NO_LEFT_ARM=true
+else
+ export MOVO_HAS_NO_LEFT_ARM=false
+fi
+if $MOVO_HAS_NO_LEFT_ARM && $MOVO_HAS_RIGHT_ARM_6DOF; then
+ export MOVO_HAS_RIGHT_6DOF_ARM_ONLY=true
+else
+ export MOVO_HAS_RIGHT_6DOF_ARM_ONLY=false
+fi
+if $MOVO_HAS_NO_LEFT_ARM && $MOVO_HAS_RIGHT_ARM_7DOF; then
+ export MOVO_HAS_RIGHT_7DOF_ARM_ONLY=true
+else
+ export MOVO_HAS_RIGHT_7DOF_ARM_ONLY=false
+fi
+if $MOVO_HAS_NO_RIGHT_ARM && $MOVO_HAS_LEFT_ARM_6DOF; then
+ export MOVO_HAS_LEFT_6DOF_ARM_ONLY=true
+else
+ export MOVO_HAS_LEFT_6DOF_ARM_ONLY=false
+fi
+if $MOVO_HAS_NO_RIGHT_ARM && $MOVO_HAS_LEFT_ARM_7DOF; then
+ export MOVO_HAS_LEFT_7DOF_ARM_ONLY=true
+else
+ export MOVO_HAS_LEFT_7DOF_ARM_ONLY=false
+fi
+
export KINOVA_RIGHT_ARM_IP_ADDRESS=10.66.171.15
export KINOVA_LEFT_ARM_IP_ADDRESS=10.66.171.16
export KINOVA_ARM_IFACE=eth0
-export MOVO_HAS_KINOVA_ARM_7DOF=false
-export MOVO_HAS_KINOVA_ARM_6DOF=true
#Camera configurations
export MOVO_HAS_KINECT_CAMERA=true
diff --git a/movo_common/movo_config/movo_config.bash~ b/movo_common/movo_config/movo_config.bash~
new file mode 100644
index 00000000..163903a9
--- /dev/null
+++ b/movo_common/movo_config/movo_config.bash~
@@ -0,0 +1,134 @@
+#!/bin/bash
+
+# This configures the environment variables for a MoVo simulation
+# This is necessary to run before starting the simulation
+#
+
+# If there is an onboard PC powered by the system this will run the watchdog
+# to make sure it gets gracefully shutdown before the system power cuts out.
+#export MOVO_POWERS_PC_ONBOARD=true
+
+unset "${!MOVO_HAS_@}"
+
+#Default Movo network
+export MOVO_IP_ADDRESS=10.66.171.5
+
+# The reference frame for all the accessories and sensors
+export MOVO_PARENT_LINK=base_chassis_link
+
+#
+# This enables the laser scan matcher which generates a /movo/lsm/pose_stamped in the odom frame
+#
+export MOVO_ENABLE_LSM=true
+
+#
+# Set this if you want the platform odometry to be corrected by LSM. This is stable indoors, but should be tested with
+# teleoperation before using it
+#
+export MOVO_USE_LSM_TO_CORRECT_ODOMETRY=true
+
+# This will run the full system tele-op node (ie can control all the joints in the system) it is not
+# collision aware and is really only meant for demonstration purposes. If set false, teleop just controls
+# the mobile platform
+export RUN_FULL_SYSTEM_TELEOP=true
+
+# Joystick configurations for joystick set MOVO_JOY_IS_ATTACHED if the joystick
+# is physically attached to this PC
+export MOVO_JOY_IS_ATTACHED=false
+export MOVO_JOY_DEV=/dev/input/js0
+
+#define if movo is wearing skins
+export MOVO_HAS_BODY=true
+
+#Front laser hardware config
+export MOVO_LASER1_IP=10.66.171.8
+export MOVO_LASER1_PORT=2112
+export LASER1_MAX_RANGE=10.0
+export LASER1_MIN_RANGE=0.01
+export LASER1_MAX_ANGLE=2.0
+export LASER1_MIN_ANGLE=-2.0
+export LASER1_PREFIX="front"
+
+#Rear laser hardware config
+export MOVO_LASER2_IP=10.66.171.9
+export MOVO_LASER2_PORT=2112
+export LASER2_MAX_RANGE=10.0
+export LASER2_MIN_RANGE=0.01
+export LASER2_MAX_ANGLE=2.0
+export LASER2_MIN_ANGLE=-2.0
+export LASER2_PREFIX="rear"
+
+#Kinova arm configurations (the right arm should be the default if there is only one)
+#export MOVO_HAS_KINOVA_ARM=true
+#export MOVO_HAS_TWO_KINOVA_ARMS=true
+
+export MOVO_HAS_RIGHT_ARM_6DOF=false
+export MOVO_HAS_RIGHT_ARM_7DOF=true
+export MOVO_HAS_LEFT_ARM_6DOF=false
+export MOVO_HAS_LEFT_ARM_7DOF=true
+
+
+# automatically set variables. do not touch
+if $MOVO_HAS_RIGHT_ARM_7DOF && $MOVO_HAS_LEFT_ARM_7DOF; then
+ export MOVO_HAS_TWO_7DOF_ARMS=true
+fi
+if $MOVO_HAS_RIGHT_ARM_6DOF && $MOVO_HAS_LEFT_ARM_6DOF; then
+ export MOVO_HAS_TWO_6DOF_ARMS=true
+fi
+if [ $MOVO_HAS_RIGHT_ARM_7DOF = false ] && [ $MOVO_HAS_RIGHT_ARM_6DOF = false ]; then
+ export MOVO_HAS_NO_RIGHT_ARM=true
+else
+ export MOVO_HAS_NO_RIGHT_ARM=false
+fi
+if [ $MOVO_HAS_LEFT_ARM_7DOF = false ] && [ $MOVO_HAS_LEFT_ARM_6DOF = false ]; then
+ export MOVO_HAS_NO_LEFT_ARM=true
+else
+ export MOVO_HAS_NO_LEFT_ARM=false
+fi
+if $MOVO_HAS_NO_LEFT_ARM && $MOVO_HAS_RIGHT_ARM_6DOF; then
+ export MOVO_HAS_RIGHT_6DOF_ARM_ONLY=true
+fi
+if $MOVO_HAS_NO_LEFT_ARM && $MOVO_HAS_RIGHT_ARM_7DOF; then
+ export MOVO_HAS_RIGHT_7DOF_ARM_ONLY=true
+fi
+if $MOVO_HAS_NO_RIGHT_ARM && $MOVO_HAS_LEFT_ARM_6DOF; then
+ export MOVO_HAS_LEFT_6DOF_ARM_ONLY=true
+fi
+if $MOVO_HAS_NO_RIGHT_ARM && $MOVO_HAS_LEFT_ARM_7DOF; then
+ export MOVO_HAS_LEFT_7DOF_ARM_ONLY=true
+fi
+
+export KINOVA_RIGHT_ARM_IP_ADDRESS=10.66.171.15
+export KINOVA_LEFT_ARM_IP_ADDRESS=10.66.171.16
+export KINOVA_ARM_IFACE=eth0
+#export MOVO_HAS_KINOVA_ARM_7DOF=true
+#export MOVO_HAS_KINOVA_ARM_6DOF=false
+
+#gripper configurations
+
+# Must set all three environment variables for on gripper
+# type to true. The moveit configurations only support the same
+# type of gripper for left and right
+
+#Kinova KG2
+export USE_KG2_FOR_MOVEIT_CONFIG=true
+export MOVO_HAS_RIGHT_KG2_GRIPPER=true
+export MOVO_HAS_LEFT_KG2_GRIPPER=true
+
+#Kinova KG3
+export USE_KG3_FOR_MOVEIT_CONFIG=false
+export MOVO_HAS_RIGHT_KG3_GRIPPER=false
+export MOVO_HAS_LEFT_KG3_GRIPPER=false
+
+#Robotiq 85 two finger gripper
+export USE_R85_FOR_MOVEIT_CONFIG=false
+export MOVO_HAS_RIGHT_ROBOTIQ_GRIPPER=false
+export MOVO_HAS_LEFT_ROBOTIQ_GRIPPER=false
+
+#export for kinect2 bridge
+export OCL_IGNORE_SELF_TEST=1
+
+#allows for ssh launch of Kinect bridge
+export DISPLAY=:0
+
+
diff --git a/movo_common/movo_description/urdf/gzplugin_grasp_fix.urdf.xacro b/movo_common/movo_description/urdf/gzplugin_grasp_fix.urdf.xacro
index 61a66fd6..009bd1fb 100644
--- a/movo_common/movo_description/urdf/gzplugin_grasp_fix.urdf.xacro
+++ b/movo_common/movo_description/urdf/gzplugin_grasp_fix.urdf.xacro
@@ -27,8 +27,12 @@ NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
+
+
+
+
-
+
@@ -54,54 +58,50 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-
+
+
-
+
- left_arm
- left_wrist_3_link
- left_gripper_finger1_knuckle_link
- left_gripper_finger1_finger_link
- left_gripper_finger1_inner_knuckle_link
- left_gripper_finger1_finger_tip_link
- left_gripper_finger2_knuckle_link
- left_gripper_finger2_finger_link
- left_gripper_finger2_inner_knuckle_link
- left_gripper_finger2_finger_tip_link
+ right_arm
+ right_wrist_3_link
+ right_gripper_finger1_knuckle_link
+ right_gripper_finger2_knuckle_link
- 90
- 100
+ 100
+ 50
1
2
- 0.005
+ 0.006
true
- left_gripper/contacts
+ right_gripper/contacts
-
-
-
+
+
right_arm
- right_wrist_3_link
- right_gripper_finger1_knuckle_link
- right_gripper_finger2_knuckle_link
+ right_wrist_3_link
+ right_gripper_finger1_knuckle_link
+ right_gripper_finger2_knuckle_link
+ right_gripper_finger3_knuckle_link
100
50
1
- 2
+ 8
0.006
true
right_gripper/contacts
+
+
@@ -122,24 +122,29 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-
+
+
-
+
- right_arm
- right_wrist_3_link
- right_gripper_finger1_knuckle_link
- right_gripper_finger2_knuckle_link
- right_gripper_finger3_knuckle_link
+ left_arm
+ left_wrist_3_link
+ left_gripper_finger1_knuckle_link
+ left_gripper_finger1_finger_link
+ left_gripper_finger1_inner_knuckle_link
+ left_gripper_finger1_finger_tip_link
+ left_gripper_finger2_knuckle_link
+ left_gripper_finger2_finger_link
+ left_gripper_finger2_inner_knuckle_link
+ left_gripper_finger2_finger_tip_link
- 100
- 50
+ 90
+ 100
1
- 8
- 0.006
+ 2
+ 0.005
true
- right_gripper/contacts
+ left_gripper/contacts
diff --git a/movo_common/movo_description/urdf/movo.urdf.xacro b/movo_common/movo_description/urdf/movo.urdf.xacro
index bcf260ce..fb543e1b 100644
--- a/movo_common/movo_description/urdf/movo.urdf.xacro
+++ b/movo_common/movo_description/urdf/movo.urdf.xacro
@@ -75,29 +75,45 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -136,7 +152,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
diff --git a/movo_common/movo_ros/bin/movo_arm_ctl b/movo_common/movo_ros/bin/movo_arm_ctl
index bb3ef388..8ad15e98 100755
--- a/movo_common/movo_ros/bin/movo_arm_ctl
+++ b/movo_common/movo_ros/bin/movo_arm_ctl
@@ -44,11 +44,12 @@ if __name__ == "__main__":
"""
rospy.init_node('movo_arm_ctl')
arm_name = rospy.get_param("~arm_name",'right')
- gripper = rospy.get_param("~gripper", '')
+ gripper = rospy.get_param("~gripper", 'none')
jaco_ip = rospy.get_param("~jaco_ip", '10.66.171.15')
interface = rospy.get_param("~interface", 'eth0')
- dof = rospy.get_param("~jaco_dof", '')
- driver = MovoArmJTAS(arm_name,gripper,interface,jaco_ip,dof)
+ dof_r = rospy.get_param("~jaco_dof_right_arm", 'none')
+ dof_l = rospy.get_param("~jaco_dof_left_arm", 'none')
+ driver = MovoArmJTAS(arm_name,gripper,interface,jaco_ip,dof_r, dof_l)
if driver.init_success:
rospy.on_shutdown(driver.clean_shutdown)
rospy.spin()
diff --git a/movo_common/movo_ros/src/movo_joint_interface/jaco_joint_controller.py b/movo_common/movo_ros/src/movo_joint_interface/jaco_joint_controller.py
index acf0bf55..71c6702b 100644
--- a/movo_common/movo_ros/src/movo_joint_interface/jaco_joint_controller.py
+++ b/movo_common/movo_ros/src/movo_joint_interface/jaco_joint_controller.py
@@ -139,6 +139,8 @@ def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.1
self._joint_fb['position'] = pos[:self._num_joints]
self._joint_fb['velocity'] = vel[:self._num_joints]
self._joint_fb['force'] = force[:self._num_joints]
+
+ self.num_fingers = 0
if ("kg2" == gripper) or ("rq85" == gripper):
self._gripper_joint_names = [self._prefix+'_gripper_finger1_joint',
@@ -625,7 +627,10 @@ def _run_ctl(self,events):
(SIArmController.TRAJECTORY == self._ctl_mode):
# Send zero angular commands to all joints and the fingers
- self.api.send_angular_vel_cmds([0.0] * (self._num_joints + self.num_fingers))
+ if self.num_fingers != 0:
+ self.api.send_angular_vel_cmds([0.0] * (self._num_joints + self.num_fingers))
+ else: # Without a gripper the API still needs 3 values to send but they will not be used
+ self.api.send_angular_vel_cmds([0.0] * (self._num_joints + 3))
else:
rospy.logerr("{} arm controller: Unrecognized control mode {}".format(
diff --git a/movo_common/movo_ros/src/movo_jtas/movo_arm_jtas.py b/movo_common/movo_ros/src/movo_jtas/movo_arm_jtas.py
index 96877695..5a36df67 100644
--- a/movo_common/movo_ros/src/movo_jtas/movo_arm_jtas.py
+++ b/movo_common/movo_ros/src/movo_jtas/movo_arm_jtas.py
@@ -87,13 +87,18 @@ def calc_grip_angle(x):
return (a)
class MovoArmJTAS(object):
- def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.15", dof="", rate=100.0):
+ def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.15", dof_r="", dof_l="", rate=100.0):
self._alive = False
self.init_success = True
self._action_name = rospy.get_name()
self._prefix = prefix
# Action Feedback/Result
+
+ if ("none" == gripper):
+ self.is_gripper_present = False
+ else:
+ self.is_gripper_present = True
if ("kg2" == gripper) or ("rq85" == gripper):
self.gripper_stall_force = 20.0
@@ -106,13 +111,15 @@ def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.1
self._gripper_stall_to = 0.7
self._gripper_pos_stall = False
self._last_movement_time = rospy.get_time()
- self.dof = dof
+ self.dof_r = dof_r
+ self.dof_l = dof_l
+ print(prefix, dof_r, dof_l, gripper)
self._planner_homing = False
"""
Define the joint names
"""
- if ("6dof" == dof):
+ if ("6dof" == dof_r and "6dof" == dof_l):
self._joint_names = [self._prefix+'_shoulder_pan_joint',
self._prefix+'_shoulder_lift_joint',
self._prefix+'_elbow_joint',
@@ -137,7 +144,46 @@ def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.1
"tilt_joint"]
self._homed = [-2.135, -0.227, -1.478, -2.083, 1.445, 1.321, 2.135, 0.227, 1.478, 2.083, -1.445, -1.321, 0.25, 0.0, 0.0]
- elif ("7dof" == dof):
+
+ elif ("6dof" == dof_r and "none" == dof_l):
+ self._joint_names = [self._prefix+'_shoulder_pan_joint',
+ self._prefix+'_shoulder_lift_joint',
+ self._prefix+'_elbow_joint',
+ self._prefix+'_wrist_1_joint',
+ self._prefix+'_wrist_2_joint',
+ self._prefix+'_wrist_3_joint']
+
+ self._body_joints = ["right_elbow_joint",
+ "right_shoulder_lift_joint",
+ "right_shoulder_pan_joint",
+ "right_wrist_1_joint",
+ "right_wrist_2_joint",
+ "right_wrist_3_joint",
+ "linear_joint",
+ "pan_joint",
+ "tilt_joint"]
+ self._homed = [-2.135, -0.227, -1.478, -2.083, 1.445, 1.321, 0.25, 0.0, 0.0]
+
+ elif ("none" == dof_r and "6dof" == dof_l):
+ self._joint_names = [self._prefix+'_shoulder_pan_joint',
+ self._prefix+'_shoulder_lift_joint',
+ self._prefix+'_elbow_joint',
+ self._prefix+'_wrist_1_joint',
+ self._prefix+'_wrist_2_joint',
+ self._prefix+'_wrist_3_joint']
+
+ self._body_joints = ["left_elbow_joint",
+ "left_shoulder_lift_joint",
+ "left_shoulder_pan_joint",
+ "left_wrist_1_joint",
+ "left_wrist_2_joint",
+ "left_wrist_3_joint",
+ "linear_joint",
+ "pan_joint",
+ "tilt_joint"]
+ self._homed = [2.135, 0.227, 1.478, 2.083, -1.445, -1.321, 0.25, 0.0, 0.0]
+
+ elif ("7dof" == dof_r and "7dof" == dof_l):
self._joint_names = [self._prefix + '_shoulder_pan_joint',
self._prefix + '_shoulder_lift_joint',
self._prefix + '_arm_half_joint',
@@ -165,6 +211,48 @@ def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.1
"tilt_joint"]
self._homed = [-1.5, -0.2, -0.15, -2.0, 2.0, -1.24, -1.1, 1.5, 0.2, 0.15, 2.0, -2.0, 1.24, 1.1, 0.25, 0, 0]
+ elif ("7dof" == dof_r and "none" == dof_l):
+ self._joint_names = [self._prefix + '_shoulder_pan_joint',
+ self._prefix + '_shoulder_lift_joint',
+ self._prefix + '_arm_half_joint',
+ self._prefix + '_elbow_joint',
+ self._prefix + '_wrist_spherical_1_joint',
+ self._prefix + '_wrist_spherical_2_joint',
+ self._prefix + '_wrist_3_joint']
+
+ self._body_joints = ["right_shoulder_pan_joint",
+ "right_shoulder_lift_joint",
+ "right_arm_half_joint",
+ "right_elbow_joint",
+ "right_wrist_spherical_1_joint",
+ "right_wrist_spherical_2_joint",
+ "right_wrist_3_joint",
+ "linear_joint",
+ "pan_joint",
+ "tilt_joint"]
+ self._homed = [-1.5, -0.2, -0.15, -2.0, 2.0, -1.24, -1.1, 0.25, 0, 0]
+
+ elif ("none" == dof_r and "7dof" == dof_l):
+ self._joint_names = [self._prefix + '_shoulder_pan_joint',
+ self._prefix + '_shoulder_lift_joint',
+ self._prefix + '_arm_half_joint',
+ self._prefix + '_elbow_joint',
+ self._prefix + '_wrist_spherical_1_joint',
+ self._prefix + '_wrist_spherical_2_joint',
+ self._prefix + '_wrist_3_joint']
+
+ self._body_joints = ["left_shoulder_pan_joint",
+ "left_shoulder_lift_joint",
+ "left_arm_half_joint",
+ "left_elbow_joint",
+ "left_wrist_spherical_1_joint",
+ "left_wrist_spherical_2_joint",
+ "left_wrist_3_joint",
+ "linear_joint",
+ "pan_joint",
+ "tilt_joint"]
+ self._homed = [1.5, 0.2, 0.15, 2.0, -2.0, 1.24, 1.1, 0.25, 0, 0]
+
else:
rospy.logerr("DoF needs to be set 6 or 7, cannot start MovoArmJTAS")
return
@@ -179,6 +267,10 @@ def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.1
self._goal_error = dict()
self._path_thresh = dict()
self._traj_smoother = TrajectorySmoother(rospy.get_name(),self._prefix)
+ if (self._prefix=="right"):
+ dof=dof_r
+ elif (self._prefix=="left"):
+ dof=dof_l
self._ctl = SIArmController(self._prefix,gripper,interface,jaco_ip, dof)
self._ctl.Pause()
self._estop_delay = 0
@@ -207,20 +299,21 @@ def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.1
self._server.start()
# Action Server
- self._gripper_server = actionlib.SimpleActionServer(
- '/movo/%s_gripper_controller/gripper_cmd'%self._prefix,
- GripperCommandAction,
- execute_cb=self._on_gripper_action,
- auto_start=False)
- self._gripper_server.start()
-
- self._gripper_action_name = '/movo/%s_gripper_controller/gripper_cmd'%self._prefix
+ if self.is_gripper_present:
+ self._gripper_server = actionlib.SimpleActionServer(
+ '/movo/%s_gripper_controller/gripper_cmd'%self._prefix,
+ GripperCommandAction,
+ execute_cb=self._on_gripper_action,
+ auto_start=False)
+ self._gripper_server.start()
+
+ self._gripper_action_name = '/movo/%s_gripper_controller/gripper_cmd'%self._prefix
- # Action Feedback/Result
- self._gripper_fdbk = GripperCommandFeedback()
- self._gripper_result = GripperCommandResult()
- self._gripper_timeout = 6.0
- self._ctl.api.InitFingers()
+ # Action Feedback/Result
+ self._gripper_fdbk = GripperCommandFeedback()
+ self._gripper_result = GripperCommandResult()
+ self._gripper_timeout = 6.0
+ self._ctl.api.InitFingers()
def _home_arm_planner(self):
if self._prefix == 'left':
diff --git a/movo_common/si_utils/src/si_utils/setup_internet_on_movo1 b/movo_common/si_utils/src/si_utils/setup_internet_on_movo1
index 917f06f9..552a0671 100755
--- a/movo_common/si_utils/src/si_utils/setup_internet_on_movo1
+++ b/movo_common/si_utils/src/si_utils/setup_internet_on_movo1
@@ -1,39 +1,40 @@
#!/bin/bash
-"""--------------------------------------------------------------------
-Copyright (c) 2018, Kinova Robotics inc.
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without modification,
-are permitted provided that the following conditions are met:
-
- * Redistributions of source code must retain the above copyright notice,
- this list of conditions and the following disclaimer.
- * Redistributions in binary form must reproduce the above copyright notice,
- this list of conditions and the following disclaimer in the documentation
- and/or other materials provided with the distribution.
- * Neither the name of the copyright holder nor the names of its contributors
- may be used to endorse or promote products derived from this software
- without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
-A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
-CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
-EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
-PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
-PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
-LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
-NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
- \file setup_internet_on_movo1
-
- \brief
-
- \Platform: Ubuntu 16.04 LTS/ROS Kinetic
---------------------------------------------------------------------"""
+# --------------------------------------------------------------------
+# Copyright (c) 2018, Kinova Robotics inc.
+#
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without modification,
+# are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of the copyright holder nor the names of its contributors
+# may be used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+# CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+# EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+# PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+# PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+# NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+# \file setup_internet_on_movo1
+#
+# \brief
+#
+# \Platform: Ubuntu 16.04 LTS/ROS Kinetic
+# --------------------------------------------------------------------
echo -e "This script will give access to the Internet to MOVO1."
echo -e "The networking changes are not permanent and will wear off after a reboot."
diff --git a/movo_demos/launch/demo/demo_show_basic.launch b/movo_demos/launch/demo/demo_show_basic.launch
index 45b7e1fa..80639d26 100644
--- a/movo_demos/launch/demo/demo_show_basic.launch
+++ b/movo_demos/launch/demo/demo_show_basic.launch
@@ -12,16 +12,42 @@
-
+
-
+
+
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_demos/launch/robot/tuck_robot.launch b/movo_demos/launch/robot/tuck_robot.launch
index 957e0b76..ac52a1b0 100644
--- a/movo_demos/launch/robot/tuck_robot.launch
+++ b/movo_demos/launch/robot/tuck_robot.launch
@@ -6,11 +6,23 @@
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_demos/launch/sim/sim_assisted_teleop.launch b/movo_demos/launch/sim/sim_assisted_teleop.launch
index d661de52..7b65d70f 100644
--- a/movo_demos/launch/sim/sim_assisted_teleop.launch
+++ b/movo_demos/launch/sim/sim_assisted_teleop.launch
@@ -12,13 +12,23 @@
name="joy_bringup" output="screen"/>
-
-
-
+
+
+
+
+
+
+
-
-
-
+
+
+
+
+
+
+
-
-
-
+
+
+
+
+
+
+
-
-
-
+
+
+
+
+
+
+
-
-
-
+
+
+
+
+
+
+
-
+
+
+
-
+
+
+
-
+
+
+
-
+
+
+
-
+
+
+
-
+
+
+
-
-
-
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_desktop/movo_viz/launch/view_robot.launch b/movo_desktop/movo_viz/launch/view_robot.launch
index f9d0a423..8f49113c 100644
--- a/movo_desktop/movo_viz/launch/view_robot.launch
+++ b/movo_desktop/movo_viz/launch/view_robot.launch
@@ -3,9 +3,17 @@
-
-
+
+
+
+
+
+
diff --git a/movo_moveit_config/config/controllers_left_only.yaml b/movo_moveit_config/config/controllers_left_only.yaml
new file mode 100644
index 00000000..a5eb25ad
--- /dev/null
+++ b/movo_moveit_config/config/controllers_left_only.yaml
@@ -0,0 +1,35 @@
+controller_list:
+ - name: "movo/left_arm_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - left_shoulder_pan_joint
+ - left_shoulder_lift_joint
+ - left_elbow_joint
+ - left_wrist_1_joint
+ - left_wrist_2_joint
+ - left_wrist_3_joint
+
+ - name: "movo/torso_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - linear_joint
+
+ - name: "movo/head_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - pan_joint
+ - tilt_joint
+
+ - name: "movo/left_gripper_controller"
+ action_ns: gripper_cmd
+ type: GripperCommand
+ default: true
+ joints:
+ - left_gripper_finger1_joint
+
diff --git a/movo_moveit_config/config/controllers_right_only.yaml b/movo_moveit_config/config/controllers_right_only.yaml
new file mode 100644
index 00000000..fe0ff4b1
--- /dev/null
+++ b/movo_moveit_config/config/controllers_right_only.yaml
@@ -0,0 +1,35 @@
+controller_list:
+ - name: "movo/right_arm_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - right_shoulder_pan_joint
+ - right_shoulder_lift_joint
+ - right_elbow_joint
+ - right_wrist_1_joint
+ - right_wrist_2_joint
+ - right_wrist_3_joint
+
+ - name: "movo/torso_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - linear_joint
+
+ - name: "movo/head_controller"
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - pan_joint
+ - tilt_joint
+
+ - name: "movo/right_gripper_controller"
+ action_ns: gripper_cmd
+ type: GripperCommand
+ default: true
+ joints:
+ - right_gripper_finger1_joint
+
diff --git a/movo_moveit_config/config/fake_controllers_left_only.yaml b/movo_moveit_config/config/fake_controllers_left_only.yaml
new file mode 100644
index 00000000..730dfa98
--- /dev/null
+++ b/movo_moveit_config/config/fake_controllers_left_only.yaml
@@ -0,0 +1,30 @@
+controller_list:
+ - name: fake_left_arm_controller
+ joints:
+ - left_shoulder_pan_joint
+ - left_shoulder_lift_joint
+ - left_elbow_joint
+ - left_wrist_1_joint
+ - left_wrist_2_joint
+ - left_wrist_3_joint
+ - name: fake_left_gripper_controller
+ joints:
+ - left_gripper_finger1_joint
+ - name: fake_torso_controller
+ joints:
+ - linear_joint
+ - name: fake_head_controller
+ joints:
+ - pan_joint
+ - tilt_joint
+ - name: fake_upper_body_controller
+ joints:
+ - linear_joint
+ - left_shoulder_pan_joint
+ - left_shoulder_lift_joint
+ - left_elbow_joint
+ - left_wrist_1_joint
+ - left_wrist_2_joint
+ - left_wrist_3_joint
+ - pan_joint
+ - tilt_joint
diff --git a/movo_moveit_config/config/fake_controllers_right_only.yaml b/movo_moveit_config/config/fake_controllers_right_only.yaml
new file mode 100644
index 00000000..b103d906
--- /dev/null
+++ b/movo_moveit_config/config/fake_controllers_right_only.yaml
@@ -0,0 +1,30 @@
+controller_list:
+ - name: fake_right_arm_controller
+ joints:
+ - right_shoulder_pan_joint
+ - right_shoulder_lift_joint
+ - right_elbow_joint
+ - right_wrist_1_joint
+ - right_wrist_2_joint
+ - right_wrist_3_joint
+ - name: fake_right_gripper_controller
+ joints:
+ - right_gripper_finger1_joint
+ - name: fake_torso_controller
+ joints:
+ - linear_joint
+ - name: fake_head_controller
+ joints:
+ - pan_joint
+ - tilt_joint
+ - name: fake_upper_body_controller
+ joints:
+ - linear_joint
+ - pan_joint
+ - tilt_joint
+ - right_shoulder_pan_joint
+ - right_shoulder_lift_joint
+ - right_elbow_joint
+ - right_wrist_1_joint
+ - right_wrist_2_joint
+ - right_wrist_3_joint
diff --git a/movo_moveit_config/config/kinematics_left_only.yaml b/movo_moveit_config/config/kinematics_left_only.yaml
new file mode 100644
index 00000000..c3e5346e
--- /dev/null
+++ b/movo_moveit_config/config/kinematics_left_only.yaml
@@ -0,0 +1,12 @@
+left_arm:
+ kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
+ kinematics_solver_attempts: 5
+ solve_type: Distance
+left_side:
+ kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
+ kinematics_solver_attempts: 5
+ solve_type: Distance
diff --git a/movo_moveit_config/config/kinematics_right_only.yaml b/movo_moveit_config/config/kinematics_right_only.yaml
new file mode 100644
index 00000000..a1489ed3
--- /dev/null
+++ b/movo_moveit_config/config/kinematics_right_only.yaml
@@ -0,0 +1,12 @@
+right_arm:
+ kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
+ kinematics_solver_attempts: 5
+ solve_type: Distance
+right_side:
+ kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
+ kinematics_solver_attempts: 5
+ solve_type: Distance
diff --git a/movo_moveit_config/config/movo_kg2_left_only.srdf b/movo_moveit_config/config/movo_kg2_left_only.srdf
new file mode 100644
index 00000000..4f230d6f
--- /dev/null
+++ b/movo_moveit_config/config/movo_kg2_left_only.srdf
@@ -0,0 +1,262 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_moveit_config/config/movo_kg2_right_only.srdf b/movo_moveit_config/config/movo_kg2_right_only.srdf
new file mode 100644
index 00000000..5c8e0a53
--- /dev/null
+++ b/movo_moveit_config/config/movo_kg2_right_only.srdf
@@ -0,0 +1,266 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_moveit_config/config/movo_kg3_left_only.srdf b/movo_moveit_config/config/movo_kg3_left_only.srdf
new file mode 100644
index 00000000..2213f3f9
--- /dev/null
+++ b/movo_moveit_config/config/movo_kg3_left_only.srdf
@@ -0,0 +1,287 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_moveit_config/config/movo_kg3_right_only.srdf b/movo_moveit_config/config/movo_kg3_right_only.srdf
new file mode 100644
index 00000000..4defdeef
--- /dev/null
+++ b/movo_moveit_config/config/movo_kg3_right_only.srdf
@@ -0,0 +1,287 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_moveit_config/config/movo_r85_left_only.srdf b/movo_moveit_config/config/movo_r85_left_only.srdf
new file mode 100644
index 00000000..5ca9a1b7
--- /dev/null
+++ b/movo_moveit_config/config/movo_r85_left_only.srdf
@@ -0,0 +1,293 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_moveit_config/config/movo_r85_right_only.srdf b/movo_moveit_config/config/movo_r85_right_only.srdf
new file mode 100644
index 00000000..54afd31f
--- /dev/null
+++ b/movo_moveit_config/config/movo_r85_right_only.srdf
@@ -0,0 +1,297 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_moveit_config/config/ompl_planning_left_only.yaml b/movo_moveit_config/config/ompl_planning_left_only.yaml
new file mode 100644
index 00000000..de4e6c1e
--- /dev/null
+++ b/movo_moveit_config/config/ompl_planning_left_only.yaml
@@ -0,0 +1,130 @@
+planner_configs:
+ SBLkConfigDefault:
+ type: geometric::SBL
+ range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+ 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
+left_arm:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(left_shoulder_pan_joint,left_shoulder_lift_joint)
+ longest_valid_segment_fraction: 0.02
+torso:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+head:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+left_side:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(linear_joint,left_shoulder_pan_joint)
+ longest_valid_segment_fraction: 0.02
+upper_body:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(left_shoulder_pan_joint)
+ longest_valid_segment_fraction: 0.02
diff --git a/movo_moveit_config/config/ompl_planning_right_only.yaml b/movo_moveit_config/config/ompl_planning_right_only.yaml
new file mode 100644
index 00000000..0cff0ae4
--- /dev/null
+++ b/movo_moveit_config/config/ompl_planning_right_only.yaml
@@ -0,0 +1,130 @@
+planner_configs:
+ SBLkConfigDefault:
+ type: geometric::SBL
+ range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+ 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
+right_arm:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(right_shoulder_pan_joint,right_shoulder_lift_joint)
+ longest_valid_segment_fraction: 0.05
+torso:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+head:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+right_side:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(linear_joint,right_shoulder_pan_joint)
+ longest_valid_segment_fraction: 0.02
+upper_body:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ projection_evaluator: joints(right_shoulder_pan_joint)
+ longest_valid_segment_fraction: 0.02
diff --git a/movo_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/movo_moveit_config/launch/fake_moveit_controller_manager.launch.xml
index 5c44512f..ab883e7d 100644
--- a/movo_moveit_config/launch/fake_moveit_controller_manager.launch.xml
+++ b/movo_moveit_config/launch/fake_moveit_controller_manager.launch.xml
@@ -4,6 +4,7 @@
-
-
+
+
+
diff --git a/movo_moveit_config/launch/movo_moveit_controller_manager.launch.xml b/movo_moveit_config/launch/movo_moveit_controller_manager.launch.xml
index 88785269..2d1e3e1b 100644
--- a/movo_moveit_config/launch/movo_moveit_controller_manager.launch.xml
+++ b/movo_moveit_config/launch/movo_moveit_controller_manager.launch.xml
@@ -4,7 +4,9 @@
-
+
+
+
-
+
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/movo_robot/movo_bringup/launch/manipulation/movo_manipulation.launch b/movo_robot/movo_bringup/launch/manipulation/movo_manipulation.launch
index 8c68eca7..b0c9504c 100644
--- a/movo_robot/movo_bringup/launch/manipulation/movo_manipulation.launch
+++ b/movo_robot/movo_bringup/launch/manipulation/movo_manipulation.launch
@@ -6,27 +6,27 @@
-
-
+
+
-
-
+
+
-
+
-
-
+
+
-
-
-
+
+
+
+
+
+
+
+
-
-
-
+
+
+
+
+
+
-
-
+
+
+
+
+
+
+
+
diff --git a/movo_robot/movo_bringup/scripts/init_robot b/movo_robot/movo_bringup/scripts/init_robot
index 9fc929a7..dccfd763 100755
--- a/movo_robot/movo_bringup/scripts/init_robot
+++ b/movo_robot/movo_bringup/scripts/init_robot
@@ -46,18 +46,30 @@ if __name__ == "__main__":
move_group = MoveGroupInterface("upper_body","base_link")
move_group.setPlannerId("RRTConnectkConfigDefault")
- lgripper = GripperActionClient('left')
- rgripper = GripperActionClient('right')
- dof = rospy.get_param('~jaco_dof')
- initialized = rospy.get_param('~initialized')
- if initialized:
- rospy.set_param('~initialized', False)
+ dof_r = rospy.get_param('~jaco_dof_right_arm')
+ dof_l = rospy.get_param('~jaco_dof_left_arm')
+ rgripper_type = rospy.get_param('~gripper_right', 'none')
+ lgripper_type = rospy.get_param('~gripper_left', 'none')
- rgripper_type = rospy.get_param('~gripper_right', 'kg3')
- lgripper_type = rospy.get_param('~gripper_left', 'kg3')
- if ("6dof" == dof):
+ rgripper_is_present = False
+ if (rgripper_type == "none"):
+ rgripper = None
+
+ elif("6dof" == dof_l or "7dof" == dof_l):
+ rgripper = GripperActionClient('right')
+ rgripper_is_present = True
+
+ lgripper_is_present = False
+ if (lgripper_type == "none"):
+ lgripper = None
+
+ elif("6dof" == dof_l or "7dof" == dof_l):
+ lgripper = GripperActionClient('left')
+ lgripper_is_present = True
+
+ if ("6dof" == dof_r and "6dof" == dof_l):
upper_body_joints = ["right_elbow_joint",
"right_shoulder_lift_joint",
"right_shoulder_pan_joint",
@@ -79,9 +91,36 @@ if __name__ == "__main__":
tucked[5] -= 1.571
if (lgripper_type == 'rq85'):
tucked[11] += 1.571
+ elif ("6dof" == dof_r):
+ upper_body_joints = ["right_elbow_joint",
+ "right_shoulder_lift_joint",
+ "right_shoulder_pan_joint",
+ "right_wrist_1_joint",
+ "right_wrist_2_joint",
+ "right_wrist_3_joint",
+ "linear_joint",
+ "pan_joint",
+ "tilt_joint"]
+ homed = [-2.135,-0.227,-1.478,-2.083,1.445,1.321,0.35,0.0,0.0]
+ tucked = [-2.8,-1.48,-1.48,0,0,1.571, 0.0371,0.0,0.0]
+ if (rgripper_type == 'rq85'):
+ tucked[5] -= 1.571
+ elif ("6dof" == dof_l):
+ upper_body_joints = ["left_elbow_joint",
+ "left_shoulder_lift_joint",
+ "left_shoulder_pan_joint",
+ "left_wrist_1_joint",
+ "left_wrist_2_joint",
+ "left_wrist_3_joint",
+ "linear_joint",
+ "pan_joint",
+ "tilt_joint"]
+ homed = [2.135,0.227,1.478,2.083,-1.445,-1.321,0.35,0.0,0.0]
+ tucked = [2.8,1.48,1.48,0,0,-1.571,0.0371,0.0,0.0]
+ if (lgripper_type == 'rq85'):
+ tucked[5] += 1.571
-
- elif ("7dof" == dof):
+ elif ("7dof" == dof_r and "7dof" == dof_l):
upper_body_joints = ["right_shoulder_pan_joint",
"right_shoulder_lift_joint",
"right_arm_half_joint",
@@ -106,6 +145,38 @@ if __name__ == "__main__":
tucked[6] -= numpy.radians(60.0)
if (lgripper_type == 'rq85'):
tucked[13] += numpy.radians(60.0)
+ elif ("7dof" == dof_r):
+ upper_body_joints = ["right_shoulder_pan_joint",
+ "right_shoulder_lift_joint",
+ "right_arm_half_joint",
+ "right_elbow_joint",
+ "right_wrist_spherical_1_joint",
+ "right_wrist_spherical_2_joint",
+ "right_wrist_3_joint",
+ "linear_joint",
+ "pan_joint",
+ "tilt_joint"]
+
+ homed = [-1.5,-0.2,-0.15,-2.0,2.0,-1.24,-1.1, 0.35,0,0]
+ tucked = [-1.6,-1.4,0.4,-2.7,0.0,0.5,-1.7, 0.04, 0, 0]
+ if (rgripper_type == 'rq85'):
+ tucked[6] -= numpy.radians(60.0)
+ elif ("7dof" == dof_l):
+ upper_body_joints = ["left_shoulder_pan_joint",
+ "left_shoulder_lift_joint",
+ "left_arm_half_joint",
+ "left_elbow_joint",
+ "left_wrist_spherical_1_joint",
+ "left_wrist_spherical_2_joint",
+ "left_wrist_3_joint",
+ "linear_joint",
+ "pan_joint",
+ "tilt_joint"]
+
+ homed = [1.5,0.2,0.15,2.0,-2.0,1.24,1.1,0.35,0,0]
+ tucked = [1.6,1.4,-0.4,2.7,0.0,-0.5, 1.7, 0.04, 0, 0]
+ if (lgripper_type == 'rq85'):
+ tucked[6] += numpy.radians(60.0)
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting process")
exit()
@@ -114,8 +185,12 @@ if __name__ == "__main__":
gripper_closed = 0.01
gripper_open = 0.165
- lgripper.command(gripper_closed)
- rgripper.command(gripper_closed)
+ if (lgripper_is_present):
+ if("6dof" == dof_l or "7dof" == dof_l):
+ lgripper.command(gripper_closed)
+ if (rgripper_is_present):
+ if("6dof" == dof_r or "7dof" == dof_r):
+ rgripper.command(gripper_closed)
success=False
while not rospy.is_shutdown() and not success:
@@ -132,9 +207,12 @@ if __name__ == "__main__":
result = move_group.moveToJointPosition(upper_body_joints, tucked, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
success = True
-
- lgripper.command(gripper_closed)
- rgripper.command(gripper_closed)
+ if (lgripper_is_present):
+ if("6dof" == dof_l or "7dof" == dof_l):
+ lgripper.command(gripper_closed)
+ if (rgripper_is_present):
+ if("6dof" == dof_r or "7dof" == dof_r):
+ rgripper.command(gripper_closed)
rospy.sleep(5)
rospy.set_param('~initialized', True)
diff --git a/movo_simulation/movo_gazebo/launch/init/init_sim.xml b/movo_simulation/movo_gazebo/launch/init/init_sim.xml
index 10de765e..7d39f26f 100644
--- a/movo_simulation/movo_gazebo/launch/init/init_sim.xml
+++ b/movo_simulation/movo_gazebo/launch/init/init_sim.xml
@@ -1,7 +1,11 @@
-
+
-
-
+
+
+
+
+
+
diff --git a/movo_simulation/movo_gazebo/launch/movo.launch b/movo_simulation/movo_gazebo/launch/movo.launch
index 91a36b61..5fd6d973 100644
--- a/movo_simulation/movo_gazebo/launch/movo.launch
+++ b/movo_simulation/movo_gazebo/launch/movo.launch
@@ -28,25 +28,61 @@
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
+