Skip to content

Various fixes #16

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
aPonza wants to merge 10 commits into
base: master
Choose a base branch
from
Draft

Various fixes #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
4468755
fix launch files including dependent launch files instead of copy-pas…
Feb 19, 2020
09355ba
set panda_mvp_grasp.py as executable
Feb 19, 2020
ab8d242
automatically detect if tensorflow or tensorflow-gpu has to be instal…
Feb 19, 2020
2cc17fc
remove deprecation warnings
Feb 19, 2020
789c04c
fix folder creation
Feb 19, 2020
b35b2b3
fix controller inheritance
Feb 19, 2020
4146937
fix typos
Feb 19, 2020
c193b01
set camera mount height from yaml config file
Feb 24, 2020
f08196d
add configuration for remembered poses grip_ready and drop_box
Feb 24, 2020
a8408b1
remove divide by zero warning
Feb 24, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 10 additions & 3 deletions franka_control_wrappers/ros_nodes/set_panda_defaults.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,16 @@
rospy.wait_for_service('/franka_control/set_EE_frame')
eef_srv = rospy.ServiceProxy('/franka_control/set_EE_frame', SetEEFrame)
eef_msg = SetEEFrameRequest()
# This is the default settings + 35mm down for the gripper
gripper_offset = 0.035
eef_msg.F_T_EE = [0.707099974155426, -0.707099974155426, 0.0, 0.0, 0.707099974155426, 0.707099974155426, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, gripper_offset + 0.10339999943971634, 1.0]

# franka_msgs.msg.FrankaState keeps track of the updated F_T_EE, so the default transform has to be hardcoded
default_F_T_EE = [0.7071, -0.7071, 0.0, 0.0, 0.7071, 0.7071, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1034, 1.0]

# Add the configured gripper height to the z-axis in the translation vector part of the transform
gripper_offset = rospy.get_param("/panda_setup/camera_mount_extra_height", 0.035)

current_F_T_EE = default_F_T_EE
current_F_T_EE[14] += gripper_offset
eef_msg.F_T_EE = current_F_T_EE

res = eef_srv.call(eef_msg).success
if not res:
Expand Down
17 changes: 17 additions & 0 deletions franka_control_wrappers/src/franka_control_wrappers/panda_commander.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,20 @@ def __init__(self, group_name=None):

self.reset_publisher = rospy.Publisher('/franka_control/error_recovery/goal', ErrorRecoveryActionGoal, queue_size=1)

def set_named_poses(self):
"""
Fetch poses used in the application via their name from the parameter server and store them in the active move_group
"""
# 1 2 3 4 5 6 7
grip_ready_joint_values_as_dict = rospy.get_param("/panda_setup/remember_joint_values/grip_ready", [-0.1706, -0.8882, -0.8514, -1.9803, -0.6403, 1.3514, 0.6152])
grip_ready_joint_values = [v for k, v in sorted(grip_ready_joint_values_as_dict.items())]
self.active_group.remember_joint_values('grip_ready', grip_ready_joint_values)

# 1 2 3 4 5 6 7
drop_box_joint_values_as_dict = rospy.get_param("/panda_setup/remember_joint_values/drop_box", [ 0.8263, 1.0896, 0.8203, -0.8825, -0.7649, 1.7213, 2.2823])
drop_box_joint_values = [v for k, v in sorted(drop_box_joint_values_as_dict.items())]
self.active_group.remember_joint_values('drop_box', drop_box_joint_values)

def print_debug_info(self):
if self.active_group:
planning_frame = self.active_group.get_planning_frame()
Expand All @@ -36,6 +50,9 @@ def print_debug_info(self):
print(self.robot.get_current_state())
print("")

def get_current_pose(self):
return self.active_group.get_current_pose().pose

def set_group(self, group_name):
"""
Set the active move group
Expand Down
6 changes: 3 additions & 3 deletions ggcnn/src/ggcnn/ggcnn.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,14 @@

import tensorflow as tf
from tensorflow.keras.models import load_model
from tensorflow.keras.backend import set_session
from tensorflow.compat.v1.keras.backend import set_session

from dougsm_helpers.timeit import TimeIt

MODEL_FILE = 'models/epoch_29_model.hdf5'
sess = tf.Session()
sess = tf.compat.v1.Session()
set_session(sess)
graph = tf.get_default_graph()
graph = tf.compat.v1.get_default_graph()
model = load_model(path.join(path.dirname(__file__), MODEL_FILE))


Expand Down
2 changes: 1 addition & 1 deletion mvp_grasping/cfg/mvp_grasp.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ grasp_entropy_node:
cost:
dist_from_best_scale: 0.15
dist_from_best_gain: 0.2
dist_from_prev_view_scale: 0.0
dist_from_prev_view_scale: 1.0
dist_from_prev_view_gain: 0.0
height:
z1: 0.55
Expand Down
18 changes: 18 additions & 0 deletions mvp_grasping/cfg/panda_setup.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
camera_mount_extra_height: 0.035
remember_joint_values:
grip_ready:
panda_joint1: -0.1706
panda_joint2: -0.8882
panda_joint3: -0.8514
panda_joint4: -1.9803
panda_joint5: -0.6403
panda_joint6: 1.3514
panda_joint7: 0.6152
drop_box:
panda_joint1: 0.8263
panda_joint2: 1.0896
panda_joint3: 0.8203
panda_joint4: -0.8825
panda_joint5: -0.7649
panda_joint6: -1.7213
panda_joint7: 2.2823
3 changes: 2 additions & 1 deletion mvp_grasping/launch/grasp_entropy_service.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
<?xml version="1.0" ?>
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE launch>
<launch>
<rosparam command="load" file="$(find mvp_grasping)/cfg/mvp_grasp.yaml" />
<node name="grasp_entropy_node" pkg="mvp_grasping" type="grasp_entropy_node.py" output="screen"/>
Expand Down
20 changes: 10 additions & 10 deletions mvp_grasping/launch/robot_bringup.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,21 +1,21 @@
<?xml version="1.0" ?>
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE launch>
<launch>

<arg name="robot_ip" default="172.16.0.2" />
<arg name="robot_ip" default="172.16.0.2" />
<arg name="launch_rviz" default="true" />
<arg name="load_gripper" default="true" />

<include file="$(find franka_control)/launch/franka_control.launch">
<arg name="robot_ip" value="$(arg robot_ip)" />

<include file="$(find panda_moveit_config)/launch/panda_control_moveit_rviz.launch">
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="launch_rviz" value="$(arg launch_rviz)"/>
<arg name="load_gripper" value="$(arg load_gripper)"/>
</include>

<!-- Set some project specific defaults for the panda arm. -->
<rosparam command="load" file="$(find mvp_grasping)/cfg/panda_setup.yaml" ns="/panda_setup"/>
<node name="set_robot_defaults" pkg="franka_control_wrappers" type="set_panda_defaults.py" respawn="false" output="screen"/>

<include file="$(find panda_moveit_config)/launch/panda_moveit.launch" />
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

<include file="$(find franka_control_wrappers)/launch/cartesian_velocity_controller.launch">
<arg name="stopped" value="true" />
</include>

</launch>
205 changes: 7 additions & 198 deletions mvp_grasping/launch/wrist_realsense.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,202 +1,13 @@
<!--
Copyright (c) 2018 Intel Corporation

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->

<!--
A launch file, derived from rgbd_launch and customized for Realsense ROS driver,
to publish XYZRGB point cloud like an OpenNI camera.

NOTICE: To use this launch file you must first install ros package rgbd_launch.

To launch Realsense with software registeration (ROS Image Pipeline and rgbd_launch):
$ roslaunch realsense2_camera rs_rgbd.launch
Processing enabled by ROS driver:
# depth rectification
Processing enabled by this node:
# rgb rectification
# depth registeration
# pointcloud_xyzrgb generation

To launch Realsense with hardware registeration (ROS Realsense depth alignment):
$ roslaunch realsense2_camera rs_rgbd.launch align_depth:=true
Processing enabled by ROS driver:
# depth rectification
# depth registration
Processing enabled by this node:
# rgb rectification
# pointcloud_xyzrgb generation
-->

<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE launch>
<launch>
<arg name="namespace" default="camera"/>
<arg name="camera" default="camera"/>
<arg name="tf_prefix" default="$(arg camera)"/>
<arg name="external_manager" default="false"/>
<arg name="manager" default="realsense2_camera_manager"/>

<!-- Camera device specific arguments -->

<arg name="serial_no" default=""/>
<arg name="usb_port_id" default=""/>
<arg name="device_type" default=""/>
<arg name="json_file_path" default=""/>

<arg name="fisheye_width" default="640"/>
<arg name="fisheye_height" default="480"/>
<arg name="enable_fisheye" default="true"/>

<arg name="depth_width" default="640"/>
<arg name="depth_height" default="480"/>
<arg name="enable_depth" default="true"/>

<arg name="infra_width" default="640"/>
<arg name="infra_height" default="480"/>
<arg name="enable_infra1" default="true"/>
<arg name="enable_infra2" default="true"/>

<arg name="color_width" default="640"/>
<arg name="color_height" default="480"/>
<arg name="enable_color" default="true"/>

<arg name="fisheye_fps" default="30"/>
<arg name="depth_fps" default="30"/>
<arg name="infra_fps" default="30"/>
<arg name="color_fps" default="30"/>
<arg name="gyro_fps" default="400"/>
<arg name="accel_fps" default="250"/>
<arg name="enable_gyro" default="true"/>
<arg name="enable_accel" default="true"/>

<arg name="enable_pointcloud" default="false"/>
<arg name="enable_sync" default="true"/>
<arg name="align_depth" default="true"/>
<arg name="filters" default="disparity,spatial,temporal"/>

<!-- rgbd_launch specific arguments -->

<!-- Arguments for remapping all device namespaces -->
<arg name="rgb" default="color" />
<arg name="ir" default="infra1" />
<arg name="depth" default="depth" />
<arg name="depth_registered_pub" default="depth_registered" />
<arg name="depth_registered" default="depth_registered" unless="$(arg align_depth)" />
<arg name="depth_registered" default="aligned_depth_to_color" if="$(arg align_depth)" />
<arg name="depth_registered_filtered" default="$(arg depth_registered)" />
<arg name="projector" default="projector" />

<!-- Disable bond topics by default -->
<arg name="bond" default="false" />
<arg name="respawn" default="$(arg bond)" />

<!-- Processing Modules -->
<arg name="rgb_processing" default="true"/>
<arg name="debayer_processing" default="false" />
<arg name="ir_processing" default="false"/>
<arg name="depth_processing" default="false"/>
<arg name="depth_registered_processing" default="true"/>
<arg name="disparity_processing" default="false"/>
<arg name="disparity_registered_processing" default="false"/>
<arg name="hw_registered_processing" default="$(arg align_depth)" />
<arg name="sw_registered_processing" default="true" unless="$(arg align_depth)" />
<arg name="sw_registered_processing" default="false" if="$(arg align_depth)" />

<group ns="$(arg camera)">

<!-- Launch the camera device nodelet-->
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
<arg name="external_manager" value="$(arg external_manager)"/>
<arg name="manager" value="$(arg manager)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="serial_no" value="$(arg serial_no)"/>
<arg name="usb_port_id" value="$(arg usb_port_id)"/>
<arg name="device_type" value="$(arg device_type)"/>
<arg name="json_file_path" value="$(arg json_file_path)"/>

<arg name="enable_pointcloud" value="$(arg enable_pointcloud)"/>
<arg name="enable_sync" value="$(arg enable_sync)"/>
<arg name="align_depth" value="$(arg align_depth)"/>

<arg name="fisheye_width" value="$(arg fisheye_width)"/>
<arg name="fisheye_height" value="$(arg fisheye_height)"/>
<arg name="enable_fisheye" value="$(arg enable_fisheye)"/>

<arg name="depth_width" value="$(arg depth_width)"/>
<arg name="depth_height" value="$(arg depth_height)"/>
<arg name="enable_depth" value="$(arg enable_depth)"/>

<arg name="color_width" value="$(arg color_width)"/>
<arg name="color_height" value="$(arg color_height)"/>
<arg name="enable_color" value="$(arg enable_color)"/>

<arg name="infra_width" value="$(arg infra_width)"/>
<arg name="infra_height" value="$(arg infra_height)"/>
<arg name="enable_infra1" value="$(arg enable_infra1)"/>
<arg name="enable_infra2" value="$(arg enable_infra2)"/>

<arg name="fisheye_fps" value="$(arg fisheye_fps)"/>
<arg name="depth_fps" value="$(arg depth_fps)"/>
<arg name="infra_fps" value="$(arg infra_fps)"/>
<arg name="color_fps" value="$(arg color_fps)"/>
<arg name="gyro_fps" value="$(arg gyro_fps)"/>
<arg name="accel_fps" value="$(arg accel_fps)"/>
<arg name="enable_gyro" value="$(arg enable_gyro)"/>
<arg name="enable_accel" value="$(arg enable_accel)"/>
<arg name="filters" value="$(arg filters)"/>
</include>

<!-- RGB processing -->
<include if="$(arg rgb_processing)"
file="$(find rgbd_launch)/launch/includes/rgb.launch.xml">
<arg name="manager" value="$(arg manager)" />
<arg name="respawn" value="$(arg respawn)" />
<arg name="rgb" value="$(arg rgb)" />
<arg name="debayer_processing" value="$(arg debayer_processing)" />
</include>

<group if="$(eval depth_registered_processing and sw_registered_processing)">
<node pkg="nodelet" type="nodelet" name="register_depth"
args="load depth_image_proc/register $(arg manager) $(arg bond)" respawn="$(arg respawn)">
<remap from="rgb/camera_info" to="$(arg rgb)/camera_info" />
<remap from="depth/camera_info" to="$(arg depth)/camera_info" />
<remap from="depth/image_rect" to="$(arg depth)/image_rect_raw" />
<remap from="depth_registered/image_rect" to="$(arg depth_registered)/sw_registered/image_rect_raw" />
</node>

<!-- Publish registered XYZRGB point cloud with software registered input -->
<node pkg="nodelet" type="nodelet" name="points_xyzrgb_sw_registered"
args="load depth_image_proc/point_cloud_xyzrgb $(arg manager) $(arg bond)" respawn="$(arg respawn)">
<remap from="rgb/image_rect_color" to="$(arg rgb)/image_rect_color" />
<remap from="rgb/camera_info" to="$(arg rgb)/camera_info" />
<remap from="depth_registered/image_rect" to="$(arg depth_registered_filtered)/sw_registered/image_rect_raw" />
<remap from="depth_registered/points" to="$(arg depth_registered)/points" />
</node>
</group>

<group if="$(eval depth_registered_processing and hw_registered_processing)">
<!-- Publish registered XYZRGB point cloud with hardware registered input (ROS Realsense depth alignment) -->
<node pkg="nodelet" type="nodelet" name="points_xyzrgb_hw_registered"
args="load depth_image_proc/point_cloud_xyzrgb $(arg manager) $(arg bond)" respawn="$(arg respawn)">
<remap from="rgb/image_rect_color" to="$(arg rgb)/image_rect_color" />
<remap from="rgb/camera_info" to="$(arg rgb)/camera_info" />
<remap from="depth_registered/image_rect" to="$(arg depth_registered)/image_raw" />
<remap from="depth_registered/points" to="$(arg depth_registered_pub)/points" />
</node>
</group>

</group>


<include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
<arg name="filters" value="$(arg filters)"/>
</include>

<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager"/>

<!-- <node pkg="nodelet" type="nodelet" name="convert_depth_to_m"
Expand Down Expand Up @@ -226,6 +37,4 @@ Processing enabled by this node:
<param name="filter_smooth_alpha" value="0.1"/>
<param name="holes_fill" value="3" />
</node>


</launch>
2 changes: 2 additions & 0 deletions mvp_grasping/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@
<exec_depend>std_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>panda_moveit_config</exec_depend>
<exec_depend>realsense2_camera</exec_depend>


<!-- The export tag contains other, unspecified, tags -->
Expand Down
2 changes: 1 addition & 1 deletion mvp_grasping/ros_nodes/grasp_entropy_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ def update_service_handler(self, req):
"""
Update the GridWorld with a new observation, compute the viewpoint entropy and generate a new command.
:param req: Ignored
:return: NextViewpointResponse (success flag, best grsap, velocity command)
:return: NextViewpointResponse (success flag, best grasp, velocity command)
"""

# Some initial checks
Expand Down
Loading