From 24616ab87aa9464a661945b86ca2822104d656b8 Mon Sep 17 00:00:00 2001 From: mblouin Date: Tue, 27 Nov 2018 16:43:33 -0500 Subject: [PATCH 01/13] modified files first implementation untested --- .../config/controllers_right_only.yaml | 36 ++ .../config/kinematics_right_only.yaml | 12 + .../config/movo_kg3_right_only.srdf | 264 +++++++++++++++ .../config/ompl_planning_right_only.yaml | 130 ++++++++ .../movo_moveit_controller_manager.launch.xml | 3 +- .../launch/ompl_planning_pipeline.launch.xml | 7 +- .../launch/planning_context.launch | 29 +- movo_common/movo_config/movo_config.bash | 34 +- .../urdf/gzplugin_grasp_fix.urdf.xacro | 81 ++--- .../movo_description/urdf/movo.urdf.xacro | 61 ++-- movo_common/movo_ros/bin/movo_arm_ctl | 5 +- .../movo_ros/src/movo_jtas/movo_arm_jtas.py | 49 ++- movo_demos/launch/demo/demo_show_basic.launch | 26 +- movo_demos/launch/robot/tuck_robot.launch | 24 +- .../launch/sim/sim_assisted_teleop.launch | 16 +- movo_demos/launch/sim/sim_demo.launch | 16 +- movo_demos/launch/sim/sim_demo_VM.launch | 20 +- .../launch/sim/sim_demo_show_basic.launch | 16 +- movo_demos/launch/sim/sim_map_nav.launch | 16 +- movo_demos/launch/sim/sim_mapping.launch | 14 +- movo_demos/launch/sim/sim_sensor_nav.launch | 14 +- movo_demos/launch/sim/sim_teleop.launch | 14 +- movo_demos/scripts/demo_show_basic.py | 311 ++++++++++++------ movo_demos/scripts/tuck_robot | 49 ++- .../movo_viz/launch/view_model.launch | 17 +- .../movo_viz/launch/view_robot.launch | 12 +- .../manipulation/movo_manipulation.launch | 14 +- .../launch/manipulation/movo_moveit.launch | 25 +- movo_robot/movo_bringup/scripts/init_robot | 65 +++- .../movo_gazebo/launch/init/init_sim.xml | 10 +- .../movo_gazebo/launch/movo.launch | 62 +++- movo_simulation/movo_gazebo/scripts/init_sim | 59 ++-- 32 files changed, 1207 insertions(+), 304 deletions(-) create mode 100644 movo_7dof_moveit_config/config/controllers_right_only.yaml create mode 100644 movo_7dof_moveit_config/config/kinematics_right_only.yaml create mode 100644 movo_7dof_moveit_config/config/movo_kg3_right_only.srdf create mode 100644 movo_7dof_moveit_config/config/ompl_planning_right_only.yaml 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/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_kg3_right_only.srdf b/movo_7dof_moveit_config/config/movo_kg3_right_only.srdf new file mode 100644 index 00000000..b544ca23 --- /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/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/movo_moveit_controller_manager.launch.xml b/movo_7dof_moveit_config/launch/movo_moveit_controller_manager.launch.xml index 17a2b4e3..6db854b7 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,8 @@ - + + + + + + diff --git a/movo_7dof_moveit_config/launch/planning_context.launch b/movo_7dof_moveit_config/launch/planning_context.launch index bcd75940..5e2380bc 100644 --- a/movo_7dof_moveit_config/launch/planning_context.launch +++ b/movo_7dof_moveit_config/launch/planning_context.launch @@ -6,21 +6,32 @@ - + - - - - - - - + + + + + + + + + + + + + + + + + + - + diff --git a/movo_common/movo_config/movo_config.bash b/movo_common/movo_config/movo_config.bash index d1385d43..c24a895b 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,33 @@ 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=false + + +# 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 +fi +if [ $MOVO_HAS_LEFT_ARM_7DOF = false ] && [ $MOVO_HAS_LEFT_ARM_6DOF = false ]; then + export MOVO_HAS_NO_LEFT_ARM=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=false -export MOVO_HAS_KINOVA_ARM_6DOF=true #Camera configurations export MOVO_HAS_KINECT_CAMERA=true @@ -88,7 +112,7 @@ export MOVO_HAS_LEFT_KG2_GRIPPER=false #Kinova KG3 export USE_KG3_FOR_MOVEIT_CONFIG=true export MOVO_HAS_RIGHT_KG3_GRIPPER=true -export MOVO_HAS_LEFT_KG3_GRIPPER=true +export MOVO_HAS_LEFT_KG3_GRIPPER=false #Robotiq 85 two finger gripper export USE_R85_FOR_MOVEIT_CONFIG=false 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..36cc89da 100755 --- a/movo_common/movo_ros/bin/movo_arm_ctl +++ b/movo_common/movo_ros/bin/movo_arm_ctl @@ -47,8 +47,9 @@ if __name__ == "__main__": gripper = rospy.get_param("~gripper", '') 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', '') + dof_l = rospy.get_param('~jaco_dof_left_arm', '') + 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_jtas/movo_arm_jtas.py b/movo_common/movo_ros/src/movo_jtas/movo_arm_jtas.py index 96877695..43981780 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,7 +87,7 @@ 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="none", dof_l="none", rate=100.0): self._alive = False self.init_success = True @@ -106,13 +106,14 @@ 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 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 +138,26 @@ 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 ("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 +185,27 @@ 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] + else: rospy.logerr("DoF needs to be set 6 or 7, cannot start MovoArmJTAS") return diff --git a/movo_demos/launch/demo/demo_show_basic.launch b/movo_demos/launch/demo/demo_show_basic.launch index 45b7e1fa..9d36d87f 100644 --- a/movo_demos/launch/demo/demo_show_basic.launch +++ b/movo_demos/launch/demo/demo_show_basic.launch @@ -12,16 +12,34 @@ - + - + + + + + + + + + + - + - + + + + + + + + + + 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_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..93a6f92b 100755 --- a/movo_robot/movo_bringup/scripts/init_robot +++ b/movo_robot/movo_bringup/scripts/init_robot @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python """-------------------------------------------------------------------- Copyright (c) 2017, Kinova Robotics inc. @@ -46,18 +46,19 @@ 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) + if (("6dof" == dof_r and "6dof" == dof_l) or ("7dof" == dof_r and "7dof" == dof_l)): + lgripper = GripperActionClient('left') + rgripper = GripperActionClient('right') + elif("6dof" == dof_r or "7dof" == dof_r) + rgripper = GripperActionClient('right') + 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', 'kg3') lgripper_type = rospy.get_param('~gripper_left', 'kg3') - if ("6dof" == dof): + if ("6dof" == dof_r and "6dof" == dof_l): upper_body_joints = ["right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", @@ -79,9 +80,22 @@ 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,2.135,0.0,0.0] + tucked = [-2.8,-1.48,-1.48,0,0,1.571, 2.8,0.0,0.0] + if (rgripper_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 +120,22 @@ 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) else: rospy.logerr("DoF needs to be set 6 or 7, aborting process") exit() @@ -114,8 +144,12 @@ if __name__ == "__main__": gripper_closed = 0.01 gripper_open = 0.165 - lgripper.command(gripper_closed) - rgripper.command(gripper_closed) + if (("6dof" == dof_r and "6dof" == dof_l) or ("7dof" == dof_r and "7dof" == dof_l)): + lgripper.command(gripper_closed) + rgripper.command(gripper_closed) + elif("6dof" == dof_r or "7dof" == dof_r) + rgripper.command(gripper_closed) + success=False while not rospy.is_shutdown() and not success: @@ -133,8 +167,11 @@ if __name__ == "__main__": if result.error_code.val == MoveItErrorCodes.SUCCESS: success = True - lgripper.command(gripper_closed) - rgripper.command(gripper_closed) + if (("6dof" == dof_r and "6dof" == dof_l) or ("7dof" == dof_r and "7dof" == dof_l)): + lgripper.command(gripper_closed) + rgripper.command(gripper_closed) + elif("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 @@ - - - - - - - - - - + + + + + + + + + + + - + + + + + Date: Wed, 28 Nov 2018 15:51:03 -0500 Subject: [PATCH 02/13] change files test on movo with one 7 dof arm --- .../movo_moveit_controller_manager.launch.xml | 2 +- .../launch/ompl_planning_pipeline.launch.xml | 4 +-- .../launch/planning_context.launch | 5 ++-- movo_common/movo_config/movo_config.bash | 13 +++++++- movo_common/movo_ros/bin/movo_arm_ctl | 4 +-- .../movo_ros/src/movo_jtas/movo_arm_jtas.py | 3 +- movo_demos/launch/demo/demo_show_basic.launch | 30 ++++++++----------- .../launch/manipulation/movo_moveit.launch | 2 +- movo_robot/movo_bringup/scripts/init_robot | 22 +++++++------- 9 files changed, 46 insertions(+), 39 deletions(-) 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 6db854b7..b3d4f4c0 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 @@ -5,7 +5,7 @@ - + - - - + diff --git a/movo_7dof_moveit_config/launch/planning_context.launch b/movo_7dof_moveit_config/launch/planning_context.launch index 5e2380bc..8cc31521 100644 --- a/movo_7dof_moveit_config/launch/planning_context.launch +++ b/movo_7dof_moveit_config/launch/planning_context.launch @@ -32,8 +32,9 @@ - - + + + diff --git a/movo_common/movo_config/movo_config.bash b/movo_common/movo_config/movo_config.bash index c24a895b..6c44e73d 100644 --- a/movo_common/movo_config/movo_config.bash +++ b/movo_common/movo_config/movo_config.bash @@ -88,7 +88,18 @@ fi if [ $MOVO_HAS_LEFT_ARM_7DOF = false ] && [ $MOVO_HAS_LEFT_ARM_6DOF = false ]; then export MOVO_HAS_NO_LEFT_ARM=true 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 diff --git a/movo_common/movo_ros/bin/movo_arm_ctl b/movo_common/movo_ros/bin/movo_arm_ctl index 36cc89da..83a517af 100755 --- a/movo_common/movo_ros/bin/movo_arm_ctl +++ b/movo_common/movo_ros/bin/movo_arm_ctl @@ -47,8 +47,8 @@ if __name__ == "__main__": gripper = rospy.get_param("~gripper", '') jaco_ip = rospy.get_param("~jaco_ip", '10.66.171.15') interface = rospy.get_param("~interface", 'eth0') - dof_r = rospy.get_param('~jaco_dof_right_arm', '') - dof_l = rospy.get_param('~jaco_dof_left_arm', '') + 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) 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 43981780..9ce6e2f8 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,7 +87,7 @@ def calc_grip_angle(x): return (a) class MovoArmJTAS(object): - def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.15", dof_r="none", dof_l="none", 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 @@ -108,6 +108,7 @@ def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.1 self._last_movement_time = rospy.get_time() self.dof_r = dof_r self.dof_l = dof_l + print(prefix, dof_r, dof_l) self._planner_homing = False """ diff --git a/movo_demos/launch/demo/demo_show_basic.launch b/movo_demos/launch/demo/demo_show_basic.launch index 9d36d87f..abd452c5 100644 --- a/movo_demos/launch/demo/demo_show_basic.launch +++ b/movo_demos/launch/demo/demo_show_basic.launch @@ -18,28 +18,24 @@ - - - - - - - - - + + + + + + + - - - - - - - - + + + + + + diff --git a/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch b/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch index 61a24fd9..725b629e 100644 --- a/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch +++ b/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch @@ -27,7 +27,7 @@ - + diff --git a/movo_robot/movo_bringup/scripts/init_robot b/movo_robot/movo_bringup/scripts/init_robot index 93a6f92b..256ad8ae 100755 --- a/movo_robot/movo_bringup/scripts/init_robot +++ b/movo_robot/movo_bringup/scripts/init_robot @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python """-------------------------------------------------------------------- Copyright (c) 2017, Kinova Robotics inc. @@ -47,17 +47,17 @@ if __name__ == "__main__": move_group = MoveGroupInterface("upper_body","base_link") move_group.setPlannerId("RRTConnectkConfigDefault") - if (("6dof" == dof_r and "6dof" == dof_l) or ("7dof" == dof_r and "7dof" == dof_l)): - lgripper = GripperActionClient('left') - rgripper = GripperActionClient('right') - elif("6dof" == dof_r or "7dof" == dof_r) - rgripper = GripperActionClient('right') - 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', 'kg3') lgripper_type = rospy.get_param('~gripper_left', 'kg3') + if (("6dof" == dof_r and "6dof" == dof_l) or ("7dof" == dof_r and "7dof" == dof_l)): + lgripper = GripperActionClient('left') + rgripper = GripperActionClient('right') + elif("6dof" == dof_r or "7dof" == dof_r): + rgripper = GripperActionClient('right') + if ("6dof" == dof_r and "6dof" == dof_l): upper_body_joints = ["right_elbow_joint", "right_shoulder_lift_joint", @@ -80,7 +80,7 @@ if __name__ == "__main__": tucked[5] -= 1.571 if (lgripper_type == 'rq85'): tucked[11] += 1.571 - elif ("6dof" == dof_r): + elif ("6dof" == dof_r): upper_body_joints = ["right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", @@ -120,7 +120,7 @@ if __name__ == "__main__": tucked[6] -= numpy.radians(60.0) if (lgripper_type == 'rq85'): tucked[13] += numpy.radians(60.0) - elif ("7dof" == dof_r): + elif ("7dof" == dof_r): upper_body_joints = ["right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", @@ -147,7 +147,7 @@ if __name__ == "__main__": if (("6dof" == dof_r and "6dof" == dof_l) or ("7dof" == dof_r and "7dof" == dof_l)): lgripper.command(gripper_closed) rgripper.command(gripper_closed) - elif("6dof" == dof_r or "7dof" == dof_r) + elif("6dof" == dof_r or "7dof" == dof_r): rgripper.command(gripper_closed) @@ -170,7 +170,7 @@ if __name__ == "__main__": if (("6dof" == dof_r and "6dof" == dof_l) or ("7dof" == dof_r and "7dof" == dof_l)): lgripper.command(gripper_closed) rgripper.command(gripper_closed) - elif("6dof" == dof_r or "7dof" == dof_r) + elif("6dof" == dof_r or "7dof" == dof_r): rgripper.command(gripper_closed) rospy.sleep(5) From 2eca8def95c58228f0abde31eb91ff63e01c89b8 Mon Sep 17 00:00:00 2001 From: mblouin Date: Wed, 28 Nov 2018 19:20:30 -0500 Subject: [PATCH 03/13] file modification working movo left arm 7 dof - functional init, functional demo IROS, functional joystick --- .../config/controllers_left_only.yaml | 36 ++ .../config/fake_controllers_left_only.yaml | 42 ++ .../config/fake_controllers_right_only.yaml | 42 ++ .../config/kinematics_left_only.yaml | 12 + .../config/movo_kg2_left_only.srdf | 242 ++++++++ .../config/movo_kg2_right_only.srdf | 248 ++++++++ .../config/movo_kg3_left_only.srdf | 269 +++++++++ .../config/movo_r85_left_only.srdf | 298 ++++++++++ .../config/movo_r85_right_only.srdf | 538 ++++++++++++++++++ .../config/ompl_planning_left_only.yaml | 130 +++++ .../fake_moveit_controller_manager.launch.xml | 5 +- .../movo_moveit_controller_manager.launch.xml | 1 + .../launch/ompl_planning_pipeline.launch.xml | 1 + .../launch/planning_context.launch | 30 +- movo_common/movo_config/movo_config.bash | 8 +- .../movo_ros/src/movo_jtas/movo_arm_jtas.py | 45 ++ movo_demos/launch/demo/demo_show_basic.launch | 12 + movo_demos/scripts/demo_show_basic.py | 174 ++++-- movo_demos/scripts/tuck_robot | 27 +- .../launch/manipulation/movo_moveit.launch | 1 + movo_robot/movo_bringup/scripts/init_robot | 43 +- movo_simulation/movo_gazebo/scripts/init_sim | 67 ++- 22 files changed, 2172 insertions(+), 99 deletions(-) create mode 100644 movo_7dof_moveit_config/config/controllers_left_only.yaml create mode 100644 movo_7dof_moveit_config/config/fake_controllers_left_only.yaml create mode 100644 movo_7dof_moveit_config/config/fake_controllers_right_only.yaml create mode 100644 movo_7dof_moveit_config/config/kinematics_left_only.yaml create mode 100644 movo_7dof_moveit_config/config/movo_kg2_left_only.srdf create mode 100644 movo_7dof_moveit_config/config/movo_kg2_right_only.srdf create mode 100644 movo_7dof_moveit_config/config/movo_kg3_left_only.srdf create mode 100644 movo_7dof_moveit_config/config/movo_r85_left_only.srdf create mode 100644 movo_7dof_moveit_config/config/movo_r85_right_only.srdf create mode 100644 movo_7dof_moveit_config/config/ompl_planning_left_only.yaml 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/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/movo_kg2_left_only.srdf b/movo_7dof_moveit_config/config/movo_kg2_left_only.srdf new file mode 100644 index 00000000..15fdc8df --- /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..32b6470c --- /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..fffec252 --- /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_r85_left_only.srdf b/movo_7dof_moveit_config/config/movo_r85_left_only.srdf new file mode 100644 index 00000000..875cc95e --- /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..c6c7f775 --- /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/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 b3d4f4c0..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 @@ -6,6 +6,7 @@ + + diff --git a/movo_7dof_moveit_config/launch/planning_context.launch b/movo_7dof_moveit_config/launch/planning_context.launch index 8cc31521..d468efc0 100644 --- a/movo_7dof_moveit_config/launch/planning_context.launch +++ b/movo_7dof_moveit_config/launch/planning_context.launch @@ -9,22 +9,27 @@ - - - + + - + - - - - + + + + - + - - - + + + + + + + + + @@ -35,6 +40,7 @@ + diff --git a/movo_common/movo_config/movo_config.bash b/movo_common/movo_config/movo_config.bash index 6c44e73d..2c3fca77 100644 --- a/movo_common/movo_config/movo_config.bash +++ b/movo_common/movo_config/movo_config.bash @@ -70,9 +70,9 @@ export LASER2_PREFIX="rear" #export MOVO_HAS_TWO_KINOVA_ARMS=true export MOVO_HAS_RIGHT_ARM_6DOF=false -export MOVO_HAS_RIGHT_ARM_7DOF=true +export MOVO_HAS_RIGHT_ARM_7DOF=false export MOVO_HAS_LEFT_ARM_6DOF=false -export MOVO_HAS_LEFT_ARM_7DOF=false +export MOVO_HAS_LEFT_ARM_7DOF=true # automatically set variables. do not touch @@ -122,8 +122,8 @@ export MOVO_HAS_LEFT_KG2_GRIPPER=false #Kinova KG3 export USE_KG3_FOR_MOVEIT_CONFIG=true -export MOVO_HAS_RIGHT_KG3_GRIPPER=true -export MOVO_HAS_LEFT_KG3_GRIPPER=false +export MOVO_HAS_RIGHT_KG3_GRIPPER=false +export MOVO_HAS_LEFT_KG3_GRIPPER=true #Robotiq 85 two finger gripper export USE_R85_FOR_MOVEIT_CONFIG=false 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 9ce6e2f8..af37c79d 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 @@ -139,6 +139,7 @@ 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 ("6dof" == dof_r and "none" == dof_l): self._joint_names = [self._prefix+'_shoulder_pan_joint', self._prefix+'_shoulder_lift_joint', @@ -158,6 +159,25 @@ 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, 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', @@ -207,6 +227,27 @@ 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, 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 @@ -221,6 +262,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 diff --git a/movo_demos/launch/demo/demo_show_basic.launch b/movo_demos/launch/demo/demo_show_basic.launch index abd452c5..80639d26 100644 --- a/movo_demos/launch/demo/demo_show_basic.launch +++ b/movo_demos/launch/demo/demo_show_basic.launch @@ -24,6 +24,12 @@ + + + + + + @@ -37,5 +43,11 @@ + + + + + + diff --git a/movo_demos/scripts/demo_show_basic.py b/movo_demos/scripts/demo_show_basic.py index d1fe8c00..97a1de4b 100755 --- a/movo_demos/scripts/demo_show_basic.py +++ b/movo_demos/scripts/demo_show_basic.py @@ -69,84 +69,124 @@ def say(_pub, _data): Publisher = rospy.Publisher("/movo/voice/text", String, queue_size=1, latch=True) rospy.sleep(2) - if "6dof" == dof_r: - movo_rarm = JacoActionClient(arm='right', dof=dof) + if ("6dof" == dof_r or "6dof" == dof_l): + # arm position definition + arm_home_deg = [85, 13, 122.5, 120, -83, -75] + + arm_greet_deg = [125, -30, 45, 120, 0, -170] # [2.18, -0.52, 0.79, 2.09, 0, -2.97] + + arm_traj_side_deg = [179, -90, 0, 0, 0, -179] # [3.12, -1.57, 0, 0, 0, -3.12] + + arm_traj_down_deg = [270, -45, -45, 90, 0, -179] # [4.71, -0.79, -0.79, 1.57, -3.12] + + arm_kiss_close_deg = [34, 45, 130, 90, -45, 0] # [0.59, 0.79, 2.27,1.57, -0.79, 0] + + arm_kiss_open_deg = [45, 45, 20, 125, -15, -15] # [0.79, 0.79, 0.35, 2.18, -0.26, -0.26] + + arm_tuck_deg = [90, 90, 160, 120, 0, 0] # [1.57, 1.57, 2.79, 2.09, 0, 0] + + arm_tuck_buff_deg = [80, 80, 150, 140, -20, 0] # [1.4, 1.4, 2.62, 2.44, -0.35, 0] + + elif ("7dof" == dof_r or "7dof" == dof_l): + + arm_home_deg = [85, 12, 9, 115, -115, 71, 63] + + arm_greet_deg = [125, -49, 45, 26, 0, 0, -75] # [2.17, -0.85, 0.82, 0.46, 0, 0, -1.3] + + arm_traj_side_deg = [172, -64, 76, -6, 0, 0, -75] # [3.0, -1.12, 1.33, -0.1, 0, 0, -1.3] + + arm_traj_down_deg = [93, 53, 0, 52, 0, 18, -85] # [1.63, 0.92, 0, 0.90, 0, 0.31, -1.48] + + arm_kiss_close_deg = [43, 46, 17, 140, 14, -36, 97] # [0.75, 0.8, 0.3, 2.45, 0.25, -0.63, 1.7] + + arm_kiss_open_deg = [47, 40, 0, 18, 0, -14, 99] # [0.82, 0.7, 0.0, 0.32, 0.0, -0.24, 1.73] + + arm_tuck_deg = [92, 86, -23, 155, 0, -29, 97] + + arm_tuck_buff_deg = [84, 75, -6, 148, 31, -7, 0] # [1.46, 1.30, -0.11, 2.58, 0.54, -0.13, 0] + + if ("6dof" == dof_r): + movo_rarm = JacoActionClient(arm='right', dof=dof_r) movo_rfinger = GripperActionClient('right') # arm position definition - rarm_home_deg = [-1.0 * x for x in larm_home_deg] + rarm_home_deg = [-1.0 * x for x in arm_home_deg] - rarm_greet_deg = [-1.0 * x for x in larm_greet_deg] + rarm_greet_deg = [-1.0 * x for x in arm_greet_deg] - rarm_traj_side_deg = [-1.0 * x for x in larm_traj_side_deg] + rarm_traj_side_deg = [-1.0 * x for x in arm_traj_side_deg] - rarm_traj_down_deg = [-1.0 * x for x in larm_traj_down_deg] + rarm_traj_down_deg = [-1.0 * x for x in arm_traj_down_deg] - rarm_kiss_close_deg = [-1.0 * x for x in larm_kiss_close_deg] + rarm_kiss_close_deg = [-1.0 * x for x in arm_kiss_close_deg] - rarm_kiss_open_deg = [-1.0 * x for x in larm_kiss_open_deg] + rarm_kiss_open_deg = [-1.0 * x for x in arm_kiss_open_deg] - rarm_tuck_deg = [-1.0 * x for x in larm_tuck_deg] + rarm_tuck_deg = [-1.0 * x for x in arm_tuck_deg] - rarm_tuck_buff_deg = [-1.0 * x for x in larm_tuck_buff_deg] + rarm_tuck_buff_deg = [-1.0 * x for x in arm_tuck_buff_deg] - if "6dof" == dof_l: - movo_larm = JacoActionClient(arm='left', dof=dof) + if ("6dof" == dof_l): + movo_larm = JacoActionClient(arm='left', dof=dof_l) movo_lfinger = GripperActionClient('left') # arm position definition - larm_home_deg = [85, 13, 122.5, 120, -83, -75] + # arm position definition + larm_home_deg = arm_home_deg - larm_greet_deg = [125, -30, 45, 120, 0, -170] # [2.18, -0.52, 0.79, 2.09, 0, -2.97] + larm_greet_deg = arm_greet_deg - larm_traj_side_deg = [179, -90, 0, 0, 0, -179] # [3.12, -1.57, 0, 0, 0, -3.12] + larm_traj_side_deg = arm_traj_side_deg - larm_traj_down_deg = [270, -45, -45, 90, 0, -179] # [4.71, -0.79, -0.79, 1.57, -3.12] + larm_traj_down_deg = arm_traj_down_deg - larm_kiss_close_deg = [34, 45, 130, 90, -45, 0] # [0.59, 0.79, 2.27,1.57, -0.79, 0] + larm_kiss_close_deg = arm_kiss_close_deg - larm_kiss_open_deg = [45, 45, 20, 125, -15, -15] # [0.79, 0.79, 0.35, 2.18, -0.26, -0.26] + larm_kiss_open_deg = arm_kiss_open_deg - larm_tuck_deg = [90, 90, 160, 120, 0, 0] # [1.57, 1.57, 2.79, 2.09, 0, 0] + larm_tuck_deg = arm_tuck_deg - larm_tuck_buff_deg = [80, 80, 150, 140, -20, 0] # [1.4, 1.4, 2.62, 2.44, -0.35, 0] - if "7dof" == dof_r: - movo_rarm = JacoActionClient(arm='right', dof=dof) + larm_tuck_buff_deg = arm_tuck_buff_deg + + if ("7dof" == dof_r): + movo_rarm = JacoActionClient(arm='right', dof=dof_r) movo_rfinger = GripperActionClient('right') # arm position definition - rarm_home_deg = [-1.0 * x for x in larm_home_deg] + rarm_home_deg = [-1.0 * x for x in arm_home_deg] - rarm_greet_deg = [-1.0 * x for x in larm_greet_deg] + rarm_greet_deg = [-1.0 * x for x in arm_greet_deg] - rarm_traj_side_deg = [-1.0 * x for x in larm_traj_side_deg] + rarm_traj_side_deg = [-1.0 * x for x in arm_traj_side_deg] - rarm_traj_down_deg = [-1.0 * x for x in larm_traj_down_deg] + rarm_traj_down_deg = [-1.0 * x for x in arm_traj_down_deg] - rarm_kiss_close_deg = [-1.0 * x for x in larm_kiss_close_deg] + rarm_kiss_close_deg = [-1.0 * x for x in arm_kiss_close_deg] - rarm_kiss_open_deg = [-1.0 * x for x in larm_kiss_open_deg] + rarm_kiss_open_deg = [-1.0 * x for x in arm_kiss_open_deg] - rarm_tuck_deg = [-1.0 * x for x in larm_tuck_deg] + rarm_tuck_deg = [-1.0 * x for x in arm_tuck_deg] - rarm_tuck_buff_deg = [-1.0 * x for x in larm_tuck_buff_deg] + rarm_tuck_buff_deg = [-1.0 * x for x in arm_tuck_buff_deg] - if "7dof" == dof_l: - movo_larm = JacoActionClient(arm='left', dof=dof) + if ("7dof" == dof_l): + movo_larm = JacoActionClient(arm='left', dof=dof_l) movo_lfinger = GripperActionClient('left') - larm_home_deg = [85, 12, 9, 115, -115, 71, 63] - larm_greet_deg = [125, -49, 45, 26, 0, 0, -75] # [2.17, -0.85, 0.82, 0.46, 0, 0, -1.3] + # arm position definition + larm_home_deg = arm_home_deg + + larm_greet_deg = arm_greet_deg - larm_traj_side_deg = [172, -64, 76, -6, 0, 0, -75] # [3.0, -1.12, 1.33, -0.1, 0, 0, -1.3] + larm_traj_side_deg = arm_traj_side_deg - larm_traj_down_deg = [93, 53, 0, 52, 0, 18, -85] # [1.63, 0.92, 0, 0.90, 0, 0.31, -1.48] + larm_traj_down_deg = arm_traj_down_deg - larm_kiss_close_deg = [43, 46, 17, 140, 14, -36, 97] # [0.75, 0.8, 0.3, 2.45, 0.25, -0.63, 1.7] + larm_kiss_close_deg = arm_kiss_close_deg - larm_kiss_open_deg = [47, 40, 0, 18, 0, -14, 99] # [0.82, 0.7, 0.0, 0.32, 0.0, -0.24, 1.73] + larm_kiss_open_deg = arm_kiss_open_deg - larm_tuck_deg = [92, 86, -23, 155, 0, -29, 97] + larm_tuck_deg = arm_tuck_deg - larm_tuck_buff_deg = [84, 75, -6, 148, 31, -7, 0] # [1.46, 1.30, -0.11, 2.58, 0.54, -0.13, 0] + larm_tuck_buff_deg = arm_tuck_buff_deg """ 1. Greeting words @@ -429,30 +469,46 @@ def say(_pub, _data): rospy.sleep(1) say(Publisher, "Now, please allow me to take some rest. I will be back with you in 5 minutes. Good bye. ") - movo_lfinger.command(0.0) - movo_rfinger.command(0.0) - movo_lfinger.wait(3) - movo_rfinger.wait(3) - - movo_larm.clear() - movo_rarm.clear() - tmp_left = rospy.wait_for_message("/movo/left_arm/joint_states", JointState) - current_larm_pos = list(tmp_left.position) - tmp_right = rospy.wait_for_message("/movo/right_arm/joint_states", JointState) - current_rarm_pos = list(tmp_right.position) + if ("7dof" == dof_l or "6dof" == dof_l): + movo_lfinger.command(0.0) + if ("7dof" == dof_r or "6dof" == dof_r): + movo_rfinger.command(0.0) + if ("7dof" == dof_l or "6dof" == dof_l): + movo_lfinger.wait(3) + if ("7dof" == dof_r or "6dof" == dof_r): + movo_rfinger.wait(3) + + if ("7dof" == dof_l or "6dof" == dof_l): + movo_larm.clear() + if ("7dof" == dof_r or "6dof" == dof_r): + movo_rarm.clear() + if ("7dof" == dof_l or "6dof" == dof_l): + tmp_left = rospy.wait_for_message("/movo/left_arm/joint_states", JointState) + current_larm_pos = list(tmp_left.position) + if ("7dof" == dof_r or "6dof" == dof_r): + tmp_right = rospy.wait_for_message("/movo/right_arm/joint_states", JointState) + current_rarm_pos = list(tmp_right.position) from_start_time = 0.0 - movo_larm.add_point(current_larm_pos, from_start_time) - movo_rarm.add_point(current_rarm_pos, from_start_time) + if ("7dof" == dof_l or "6dof" == dof_l): + movo_larm.add_point(current_larm_pos, from_start_time) + if ("7dof" == dof_r or "6dof" == dof_r): + movo_rarm.add_point(current_rarm_pos, from_start_time) from_start_time += 5.0 - movo_larm.add_point_deg(larm_tuck_buff_deg, from_start_time) - movo_rarm.add_point_deg(rarm_tuck_buff_deg, from_start_time) + if ("7dof" == dof_l or "6dof" == dof_l): + movo_larm.add_point_deg(larm_tuck_buff_deg, from_start_time) + if ("7dof" == dof_r or "6dof" == dof_r): + movo_rarm.add_point_deg(rarm_tuck_buff_deg, from_start_time) - movo_larm.start() - movo_rarm.start() - movo_larm.wait(from_start_time+2) - movo_rarm.wait(from_start_time+2) + if ("7dof" == dof_l or "6dof" == dof_l): + movo_larm.start() + if ("7dof" == dof_r or "6dof" == dof_r): + movo_rarm.start() + if ("7dof" == dof_l or "6dof" == dof_l): + movo_larm.wait(from_start_time+2) + if ("7dof" == dof_r or "6dof" == dof_r): + movo_rarm.wait(from_start_time+2) print("Return to Home position for next loop") process_stop_time = dt.datetime.now() diff --git a/movo_demos/scripts/tuck_robot b/movo_demos/scripts/tuck_robot index 0cf95241..0b9b007d 100755 --- a/movo_demos/scripts/tuck_robot +++ b/movo_demos/scripts/tuck_robot @@ -49,9 +49,9 @@ if __name__ == "__main__": dof_r = rospy.get_param('~jaco_dof_right_arm') dof_l = rospy.get_param('~jaco_dof_left_arm') - if ("7dof" == dof_l and "6dof" == dof_l): + if ("7dof" == dof_l or "6dof" == dof_l): lgripper = GripperActionClient('left') - if ("7dof" == dof_r and "6dof" == dof_r): + if ("7dof" == dof_r or "6dof" == dof_r): rgripper = GripperActionClient('right') if ("6dof" == dof_r and "6dof" == dof_l): @@ -82,6 +82,17 @@ if __name__ == "__main__": "pan_joint", "tilt_joint"] tucked = [-2.8, -1.48, -1.48, 0, 0, 1.571, 0.0371, 0.0, 0.0] + elif ("none" == dof_r and "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"] + tucked = [2.8, 1.48, 1.48, 0, 0, -1.571, 0.0371, 0.0, 0.0] if ("7dof" == dof_r and "7dof" == dof_l): upper_body_joints = ["right_shoulder_lift_joint", @@ -116,7 +127,19 @@ if __name__ == "__main__": "tilt_joint"] tucked = [-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -2.7, 0.04, 0, 0] + elif ("none" == dof_r and "7dof" == dof_l): + upper_body_joints = ["left_shoulder_lift_joint", + "left_shoulder_pan_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"] + tucked = [1.6, 1.5, -0.4, 2.7, 0.0, -0.5, 2.7, 0.04, 0, 0] # This is not simulation so need to adjust gripper parameters gripper_closed = 0.01 gripper_open = 0.165 diff --git a/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch b/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch index 725b629e..4df0e654 100644 --- a/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch +++ b/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch @@ -28,6 +28,7 @@ + diff --git a/movo_robot/movo_bringup/scripts/init_robot b/movo_robot/movo_bringup/scripts/init_robot index 256ad8ae..a7bbfaff 100755 --- a/movo_robot/movo_bringup/scripts/init_robot +++ b/movo_robot/movo_bringup/scripts/init_robot @@ -52,10 +52,9 @@ if __name__ == "__main__": rgripper_type = rospy.get_param('~gripper_right', 'kg3') lgripper_type = rospy.get_param('~gripper_left', 'kg3') - if (("6dof" == dof_r and "6dof" == dof_l) or ("7dof" == dof_r and "7dof" == dof_l)): + if("6dof" == dof_l or "7dof" == dof_l): lgripper = GripperActionClient('left') - rgripper = GripperActionClient('right') - elif("6dof" == dof_r or "7dof" == dof_r): + if("6dof" == dof_r or "7dof" == dof_r): rgripper = GripperActionClient('right') if ("6dof" == dof_r and "6dof" == dof_l): @@ -94,6 +93,20 @@ if __name__ == "__main__": tucked = [-2.8,-1.48,-1.48,0,0,1.571, 2.8,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_r and "7dof" == dof_l): upper_body_joints = ["right_shoulder_pan_joint", @@ -136,6 +149,22 @@ if __name__ == "__main__": 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() @@ -144,10 +173,9 @@ if __name__ == "__main__": gripper_closed = 0.01 gripper_open = 0.165 - if (("6dof" == dof_r and "6dof" == dof_l) or ("7dof" == dof_r and "7dof" == dof_l)): + if("6dof" == dof_l or "7dof" == dof_l): lgripper.command(gripper_closed) - rgripper.command(gripper_closed) - elif("6dof" == dof_r or "7dof" == dof_r): + if("6dof" == dof_r or "7dof" == dof_r): rgripper.command(gripper_closed) @@ -167,9 +195,8 @@ if __name__ == "__main__": if result.error_code.val == MoveItErrorCodes.SUCCESS: success = True - if (("6dof" == dof_r and "6dof" == dof_l) or ("7dof" == dof_r and "7dof" == dof_l)): + if("6dof" == dof_l or "7dof" == dof_l): lgripper.command(gripper_closed) - rgripper.command(gripper_closed) elif("6dof" == dof_r or "7dof" == dof_r): rgripper.command(gripper_closed) diff --git a/movo_simulation/movo_gazebo/scripts/init_sim b/movo_simulation/movo_gazebo/scripts/init_sim index 46a169db..80431c14 100755 --- a/movo_simulation/movo_gazebo/scripts/init_sim +++ b/movo_simulation/movo_gazebo/scripts/init_sim @@ -54,17 +54,16 @@ if __name__ == "__main__": move_group = MoveGroupInterface("upper_body","base_link") move_group.setPlannerId("RRTConnectkConfigDefault") - lgripper = GripperActionClient('left') - rgripper = GripperActionClient('right') - - move_group = MoveGroupInterface("upper_body","base_link") - move_group.setPlannerId("RRTConnectkConfigDefault") - lgripper = GripperActionClient('left') - rgripper = GripperActionClient('right') - + dof_r = rospy.get_param('~jaco_dof_right_arm') dof_l = rospy.get_param('~jaco_dof_left_arm') + if ("7dof" == dof_l or "6dof" == dof_l): + lgripper = GripperActionClient('left') + if ("7dof" == dof_r or "6dof" == dof_r): + rgripper = GripperActionClient('right') + + if ("6dof" == dof_r and "6dof" == dof_l) : upper_body_joints = ["right_elbow_joint", "right_shoulder_lift_joint", @@ -84,6 +83,32 @@ if __name__ == "__main__": 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.35,0.0,0.0] tucked = [-2.8,-1.48,-1.48,0,0,1.571,2.8,1.48,1.48,0,0,-1.571,0.0371,0.0,0.0] + if ("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 ("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] + elif ("7dof" == dof_r and "7dof" == dof_l): upper_body_joints = ["right_shoulder_pan_joint", "right_shoulder_lift_joint", @@ -119,6 +144,19 @@ if __name__ == "__main__": homed = [-1.5,-0.2,-0.15,-2.0,2.0,-1.24,0.35,0,0] tucked = [-1.6,-1.5,0.4,-2.7,0.0,0.5, -1.7, 0.04, 0, 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.5,-0.4,2.7,0.0,-0.5, 1.7, 0.04, 0, 0] else: rospy.logerr("DoF needs to be set 6 or 7, aborting process") @@ -133,16 +171,21 @@ if __name__ == "__main__": result = move_group.moveToJointPosition(upper_body_joints, homed, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: success=True - lgripper.command(gripper_open) - rgripper.command(gripper_open) + if ("7dof" == dof_l or "6dof" == dof_l): + lgripper.command(gripper_open) + if ("7dof" == dof_r or "6dof" == dof_r): + rgripper.command(gripper_open) success=False while not rospy.is_shutdown() and not success: 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 ("7dof" == dof_l or "6dof" == dof_l): + lgripper.command(gripper_closed) + if ("7dof" == dof_r or "6dof" == dof_r): + rgripper.command(gripper_closed) pub = rospy.Publisher('/sim_initialized',Bool,queue_size=1,latch=True) tmp = Bool(True) From 132e98ee17deea4f29ffe1b67d2678b0aded0478 Mon Sep 17 00:00:00 2001 From: mblouin Date: Thu, 29 Nov 2018 17:46:10 -0500 Subject: [PATCH 04/13] changes for a 6 dof robot - untested --- .../config/controllers_left_only.yaml | 35 +++ .../config/controllers_right_only.yaml | 35 +++ .../config/fake_controllers_left_only.yaml | 30 ++ .../config/fake_controllers_right_only.yaml | 30 ++ .../config/kinematics_left_only.yaml | 12 + .../config/kinematics_right_only.yaml | 12 + .../config/movo_kg2_left_only.srdf | 262 +++++++++++++++ .../config/movo_kg2_right_only.srdf | 266 ++++++++++++++++ .../config/movo_kg3_left_only.srdf | 287 +++++++++++++++++ .../config/movo_kg3_right_only.srdf | 287 +++++++++++++++++ .../config/movo_r85_left_only.srdf | 293 +++++++++++++++++ .../config/movo_r85_right_only.srdf | 297 ++++++++++++++++++ .../config/ompl_planning_left_only.yaml | 130 ++++++++ .../config/ompl_planning_right_only.yaml | 130 ++++++++ .../fake_moveit_controller_manager.launch.xml | 5 +- .../movo_moveit_controller_manager.launch.xml | 4 +- .../launch/ompl_planning_pipeline.launch.xml | 5 +- .../launch/planning_context.launch | 27 +- 18 files changed, 2137 insertions(+), 10 deletions(-) create mode 100644 movo_moveit_config/config/controllers_left_only.yaml create mode 100644 movo_moveit_config/config/controllers_right_only.yaml create mode 100644 movo_moveit_config/config/fake_controllers_left_only.yaml create mode 100644 movo_moveit_config/config/fake_controllers_right_only.yaml create mode 100644 movo_moveit_config/config/kinematics_left_only.yaml create mode 100644 movo_moveit_config/config/kinematics_right_only.yaml create mode 100644 movo_moveit_config/config/movo_kg2_left_only.srdf create mode 100644 movo_moveit_config/config/movo_kg2_right_only.srdf create mode 100644 movo_moveit_config/config/movo_kg3_left_only.srdf create mode 100644 movo_moveit_config/config/movo_kg3_right_only.srdf create mode 100644 movo_moveit_config/config/movo_r85_left_only.srdf create mode 100644 movo_moveit_config/config/movo_r85_right_only.srdf create mode 100644 movo_moveit_config/config/ompl_planning_left_only.yaml create mode 100644 movo_moveit_config/config/ompl_planning_right_only.yaml 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..e1bf4f32 --- /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..3fc3cc55 --- /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..4c6d93e8 --- /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..3fd09b48 --- /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..a4aec611 --- /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..830257f7 --- /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 @@ - + + + - - - - - + + + + + + + + + + + + + + + + + + + + + + From 54977e4e9417fc2696d94977eca83a015af4d682 Mon Sep 17 00:00:00 2001 From: martine1406 Date: Tue, 4 Dec 2018 15:54:12 -0500 Subject: [PATCH 05/13] fixes in the config file and planning_context file loading the urdf for 6 dof robot --- CMakeLists.txt | 1 + movo_common/movo_config/movo_config.bash | 8 +- movo_common/movo_config/movo_config.bash~ | 134 ++++++++++++++++++ .../launch/planning_context.launch | 2 +- 4 files changed, 142 insertions(+), 3 deletions(-) create mode 120000 CMakeLists.txt create mode 100644 movo_common/movo_config/movo_config.bash~ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 120000 index 00000000..3703df4e --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/indigo/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/movo_common/movo_config/movo_config.bash b/movo_common/movo_config/movo_config.bash index 2c3fca77..27549a1f 100644 --- a/movo_common/movo_config/movo_config.bash +++ b/movo_common/movo_config/movo_config.bash @@ -70,7 +70,7 @@ export LASER2_PREFIX="rear" #export MOVO_HAS_TWO_KINOVA_ARMS=true export MOVO_HAS_RIGHT_ARM_6DOF=false -export MOVO_HAS_RIGHT_ARM_7DOF=false +export MOVO_HAS_RIGHT_ARM_7DOF=true export MOVO_HAS_LEFT_ARM_6DOF=false export MOVO_HAS_LEFT_ARM_7DOF=true @@ -84,9 +84,13 @@ if $MOVO_HAS_RIGHT_ARM_6DOF && $MOVO_HAS_LEFT_ARM_6DOF; then 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 @@ -122,7 +126,7 @@ export MOVO_HAS_LEFT_KG2_GRIPPER=false #Kinova KG3 export USE_KG3_FOR_MOVEIT_CONFIG=true -export MOVO_HAS_RIGHT_KG3_GRIPPER=false +export MOVO_HAS_RIGHT_KG3_GRIPPER=true export MOVO_HAS_LEFT_KG3_GRIPPER=true #Robotiq 85 two finger gripper 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_moveit_config/launch/planning_context.launch b/movo_moveit_config/launch/planning_context.launch index 00fc3fdd..6170346c 100644 --- a/movo_moveit_config/launch/planning_context.launch +++ b/movo_moveit_config/launch/planning_context.launch @@ -6,7 +6,7 @@ - + From 1ed189f8571398d1509966c805d6fc053d08958f Mon Sep 17 00:00:00 2001 From: martine1406 Date: Wed, 5 Dec 2018 14:54:41 -0500 Subject: [PATCH 06/13] MOVO_HAS env variable set to false if appropriate condition is not met. before, those variables were just not set --- movo_common/movo_config/movo_config.bash | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/movo_common/movo_config/movo_config.bash b/movo_common/movo_config/movo_config.bash index 27549a1f..c3d9c32b 100644 --- a/movo_common/movo_config/movo_config.bash +++ b/movo_common/movo_config/movo_config.bash @@ -78,9 +78,13 @@ 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 @@ -94,15 +98,23 @@ else 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 From 5ec76c9786b7c673f34cfd5ef756d60936f0f5ac Mon Sep 17 00:00:00 2001 From: MOVO2 Date: Wed, 7 Aug 2019 17:18:35 -0400 Subject: [PATCH 07/13] Remove the top level CMakeLists file --- CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) delete mode 120000 CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 120000 index 3703df4e..00000000 --- a/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -/opt/ros/indigo/share/catkin/cmake/toplevel.cmake \ No newline at end of file From eba9eb6407cfc4e9871c69fe2135ef4c540caf78 Mon Sep 17 00:00:00 2001 From: MOVO2 Date: Wed, 7 Aug 2019 17:22:18 -0400 Subject: [PATCH 08/13] Fix copyright comment in setup internet to movo1 script --- .../src/si_utils/setup_internet_on_movo1 | 69 ++++++++++--------- 1 file changed, 35 insertions(+), 34 deletions(-) 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." From ac07ab5f369fd7f2af4b4bdfc7e9e24eb4d789a2 Mon Sep 17 00:00:00 2001 From: MOVO2 Date: Wed, 7 Aug 2019 18:31:29 -0400 Subject: [PATCH 09/13] Replace kinect link with movo camera link in move it configs --- .../config/movo_kg2_left_only.srdf | 24 ++++++++-------- .../config/movo_kg2_right_only.srdf | 24 ++++++++-------- .../config/movo_kg3_left_only.srdf | 24 ++++++++-------- .../config/movo_kg3_right_only.srdf | 24 ++++++++-------- .../config/movo_r85_left_only.srdf | 24 ++++++++-------- .../config/movo_r85_right_only.srdf | 28 +++++++++---------- .../config/movo_kg2_left_only.srdf | 24 ++++++++-------- .../config/movo_kg2_right_only.srdf | 24 ++++++++-------- .../config/movo_kg3_left_only.srdf | 24 ++++++++-------- .../config/movo_kg3_right_only.srdf | 24 ++++++++-------- .../config/movo_r85_left_only.srdf | 24 ++++++++-------- .../config/movo_r85_right_only.srdf | 24 ++++++++-------- 12 files changed, 146 insertions(+), 146 deletions(-) diff --git a/movo_7dof_moveit_config/config/movo_kg2_left_only.srdf b/movo_7dof_moveit_config/config/movo_kg2_left_only.srdf index 15fdc8df..785aee88 100644 --- a/movo_7dof_moveit_config/config/movo_kg2_left_only.srdf +++ b/movo_7dof_moveit_config/config/movo_kg2_left_only.srdf @@ -104,7 +104,7 @@ - + @@ -118,7 +118,7 @@ - + @@ -132,16 +132,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/movo_7dof_moveit_config/config/movo_kg2_right_only.srdf b/movo_7dof_moveit_config/config/movo_kg2_right_only.srdf index 32b6470c..2e29932f 100644 --- a/movo_7dof_moveit_config/config/movo_kg2_right_only.srdf +++ b/movo_7dof_moveit_config/config/movo_kg2_right_only.srdf @@ -104,7 +104,7 @@ - + @@ -118,7 +118,7 @@ - + @@ -132,16 +132,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/movo_7dof_moveit_config/config/movo_kg3_left_only.srdf b/movo_7dof_moveit_config/config/movo_kg3_left_only.srdf index fffec252..886f1ec3 100644 --- a/movo_7dof_moveit_config/config/movo_kg3_left_only.srdf +++ b/movo_7dof_moveit_config/config/movo_kg3_left_only.srdf @@ -107,7 +107,7 @@ - + @@ -121,7 +121,7 @@ - + @@ -135,16 +135,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/movo_7dof_moveit_config/config/movo_kg3_right_only.srdf b/movo_7dof_moveit_config/config/movo_kg3_right_only.srdf index b544ca23..c43eaa45 100644 --- a/movo_7dof_moveit_config/config/movo_kg3_right_only.srdf +++ b/movo_7dof_moveit_config/config/movo_kg3_right_only.srdf @@ -107,7 +107,7 @@ - + @@ -121,7 +121,7 @@ - + @@ -135,16 +135,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/movo_7dof_moveit_config/config/movo_r85_left_only.srdf b/movo_7dof_moveit_config/config/movo_r85_left_only.srdf index 875cc95e..48fcf6ab 100644 --- a/movo_7dof_moveit_config/config/movo_r85_left_only.srdf +++ b/movo_7dof_moveit_config/config/movo_r85_left_only.srdf @@ -104,7 +104,7 @@ - + @@ -118,7 +118,7 @@ - + @@ -132,16 +132,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/movo_7dof_moveit_config/config/movo_r85_right_only.srdf b/movo_7dof_moveit_config/config/movo_r85_right_only.srdf index c6c7f775..36e9519a 100644 --- a/movo_7dof_moveit_config/config/movo_r85_right_only.srdf +++ b/movo_7dof_moveit_config/config/movo_r85_right_only.srdf @@ -161,7 +161,7 @@ - + @@ -179,7 +179,7 @@ - + @@ -197,18 +197,18 @@ - - - - - - - - - - - - + + + + + + + + + + + + diff --git a/movo_moveit_config/config/movo_kg2_left_only.srdf b/movo_moveit_config/config/movo_kg2_left_only.srdf index e1bf4f32..4f230d6f 100644 --- a/movo_moveit_config/config/movo_kg2_left_only.srdf +++ b/movo_moveit_config/config/movo_kg2_left_only.srdf @@ -124,7 +124,7 @@ - + @@ -137,7 +137,7 @@ - + @@ -150,16 +150,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/movo_moveit_config/config/movo_kg2_right_only.srdf b/movo_moveit_config/config/movo_kg2_right_only.srdf index 3fc3cc55..5c8e0a53 100644 --- a/movo_moveit_config/config/movo_kg2_right_only.srdf +++ b/movo_moveit_config/config/movo_kg2_right_only.srdf @@ -124,7 +124,7 @@ - + @@ -137,7 +137,7 @@ - + @@ -150,16 +150,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/movo_moveit_config/config/movo_kg3_left_only.srdf b/movo_moveit_config/config/movo_kg3_left_only.srdf index 4c6d93e8..2213f3f9 100644 --- a/movo_moveit_config/config/movo_kg3_left_only.srdf +++ b/movo_moveit_config/config/movo_kg3_left_only.srdf @@ -128,7 +128,7 @@ - + @@ -141,7 +141,7 @@ - + @@ -154,16 +154,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/movo_moveit_config/config/movo_kg3_right_only.srdf b/movo_moveit_config/config/movo_kg3_right_only.srdf index 3fd09b48..4defdeef 100644 --- a/movo_moveit_config/config/movo_kg3_right_only.srdf +++ b/movo_moveit_config/config/movo_kg3_right_only.srdf @@ -128,7 +128,7 @@ - + @@ -141,7 +141,7 @@ - + @@ -154,16 +154,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/movo_moveit_config/config/movo_r85_left_only.srdf b/movo_moveit_config/config/movo_r85_left_only.srdf index a4aec611..5ca9a1b7 100644 --- a/movo_moveit_config/config/movo_r85_left_only.srdf +++ b/movo_moveit_config/config/movo_r85_left_only.srdf @@ -101,7 +101,7 @@ - + @@ -114,7 +114,7 @@ - + @@ -127,16 +127,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/movo_moveit_config/config/movo_r85_right_only.srdf b/movo_moveit_config/config/movo_r85_right_only.srdf index 830257f7..54afd31f 100644 --- a/movo_moveit_config/config/movo_r85_right_only.srdf +++ b/movo_moveit_config/config/movo_r85_right_only.srdf @@ -101,7 +101,7 @@ - + @@ -114,7 +114,7 @@ - + @@ -127,16 +127,16 @@ - - - - - - - - - - + + + + + + + + + + From d811b7951d74b2f6665b111af800a29da05c40eb Mon Sep 17 00:00:00 2001 From: MOVO2 Date: Wed, 7 Aug 2019 18:35:33 -0400 Subject: [PATCH 10/13] Fix a starting position in the init robot script --- movo_robot/movo_bringup/scripts/init_robot | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/movo_robot/movo_bringup/scripts/init_robot b/movo_robot/movo_bringup/scripts/init_robot index a7bbfaff..d0617227 100755 --- a/movo_robot/movo_bringup/scripts/init_robot +++ b/movo_robot/movo_bringup/scripts/init_robot @@ -89,8 +89,8 @@ if __name__ == "__main__": "linear_joint", "pan_joint", "tilt_joint"] - homed = [-2.135,-0.227,-1.478,-2.083,1.445,1.321,2.135,0.0,0.0] - tucked = [-2.8,-1.48,-1.48,0,0,1.571, 2.8,0.0,0.0] + 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): From f06e7ba227ed59c8418ffbc0de646589eaf0b9ce Mon Sep 17 00:00:00 2001 From: MOVO2 Date: Wed, 7 Aug 2019 19:21:27 -0400 Subject: [PATCH 11/13] Try to init the arm driver without a gripper - untested --- movo_common/movo_ros/bin/movo_arm_ctl | 2 +- .../jaco_joint_controller.py | 2 ++ .../movo_ros/src/movo_jtas/movo_arm_jtas.py | 34 +++++++++++-------- 3 files changed, 23 insertions(+), 15 deletions(-) diff --git a/movo_common/movo_ros/bin/movo_arm_ctl b/movo_common/movo_ros/bin/movo_arm_ctl index 83a517af..8ad15e98 100755 --- a/movo_common/movo_ros/bin/movo_arm_ctl +++ b/movo_common/movo_ros/bin/movo_arm_ctl @@ -44,7 +44,7 @@ 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_r = rospy.get_param("~jaco_dof_right_arm", 'none') 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..126c17e5 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', 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 af37c79d..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 @@ -94,6 +94,11 @@ def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.1 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 @@ -108,7 +113,7 @@ def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.1 self._last_movement_time = rospy.get_time() self.dof_r = dof_r self.dof_l = dof_l - print(prefix, dof_r, dof_l) + print(prefix, dof_r, dof_l, gripper) self._planner_homing = False """ @@ -294,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': From 28e7260c1c78504882ce700d6fa9620fed51b700 Mon Sep 17 00:00:00 2001 From: MOVO2 Date: Wed, 7 Aug 2019 19:56:08 -0400 Subject: [PATCH 12/13] Add dummy values to send to the arm if number of fingers is 0 --- .../src/movo_joint_interface/jaco_joint_controller.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 126c17e5..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 @@ -627,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( From 041fa9f8a24318a2a974316a73180ae98af681db Mon Sep 17 00:00:00 2001 From: MOVO2 Date: Wed, 7 Aug 2019 20:45:49 -0400 Subject: [PATCH 13/13] MoveIt succeeds to initialize with warnings with a missing gripper and the init script succeeds now instead of crashing --- .../launch/manipulation/movo_moveit.launch | 4 ++ movo_robot/movo_bringup/scripts/init_robot | 44 ++++++++++++------- 2 files changed, 33 insertions(+), 15 deletions(-) diff --git a/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch b/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch index 4df0e654..192afe7b 100644 --- a/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch +++ b/movo_robot/movo_bringup/launch/manipulation/movo_moveit.launch @@ -25,6 +25,10 @@ + + + + diff --git a/movo_robot/movo_bringup/scripts/init_robot b/movo_robot/movo_bringup/scripts/init_robot index d0617227..dccfd763 100755 --- a/movo_robot/movo_bringup/scripts/init_robot +++ b/movo_robot/movo_bringup/scripts/init_robot @@ -49,13 +49,25 @@ if __name__ == "__main__": 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', 'kg3') - lgripper_type = rospy.get_param('~gripper_left', 'kg3') + rgripper_type = rospy.get_param('~gripper_right', 'none') + lgripper_type = rospy.get_param('~gripper_left', 'none') - if("6dof" == dof_l or "7dof" == dof_l): - lgripper = GripperActionClient('left') - if("6dof" == dof_r or "7dof" == dof_r): + + 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", @@ -173,11 +185,12 @@ if __name__ == "__main__": gripper_closed = 0.01 gripper_open = 0.165 - if("6dof" == dof_l or "7dof" == dof_l): - lgripper.command(gripper_closed) - if("6dof" == dof_r or "7dof" == dof_r): - 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: @@ -194,11 +207,12 @@ if __name__ == "__main__": result = move_group.moveToJointPosition(upper_body_joints, tucked, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: success = True - - if("6dof" == dof_l or "7dof" == dof_l): - lgripper.command(gripper_closed) - elif("6dof" == dof_r or "7dof" == dof_r): - 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)