Body information srv and collision in ros 1

.gitignore

@@ -1,4 +1,3 @@
-
 # Byte-compiled / optimized / DLL files
 __pycache__/
 *.py[cod]
@@ -19,4 +18,10 @@ __pycache__/
 *.swo
 *.swp
 
-cmake-build-debug/
+cmake-build-debug/
+
+*__pycache__/
+*build/
+*install/
+*log/
+*vscode/

README.md

@@ -0,0 +1,31 @@
+# Roboy in pybullet
+
+## Setup 
+*Tested on Ubuntu 18.04*
+- install python>=3.6
+- install [pybullet](https://github.com/bulletphysics/bullet3/blob/master/README.md#pybullet)
+- install [ROS Melodic Morenia](http://wiki.ros.org/melodic/Installation/Ubuntu)
+
+```bash
+# setup ROS for python3
+sudo pip3 install rospkg catkin_pkg
+
+# install other requirements
+pip3 install --user pyquaternion
+pip3 install pyyaml
+
+# Create ROS workspace
+mkdir -p roboy_ws/src
+
+# Clone repos to src directory
+cd roboy_ws/src
+git clone https://github.com/Roboy/roboy3_models.git -b caspr
+git clone https://github.com/Roboy/roboy_communication.git
+git clone https://github.com/Roboy/bulletroboy.git -b simulated-animus
+
+cd roboy_ws
+catkin_make
+source devel/setup.bash
+rosrun bulletroboy ik.py
+```
+One can move the robot with the mouse pointer or sending inverse kinematics targets on the ROS topic `/bullet_ik`

src/cage_interaction.py

@@ -0,0 +1,226 @@
+import rospy
+from roboy_simulation_msgs.msg import Collision, ContactPoint, LinkInformation, BodyInformation
+from roboy_simulation_msgs.srv import UpdateBodyInformation
+from sensor_msgs.msg import JointState
+import pybullet as p
+import yaml
+import numpy as np
+from rospkg import RosPack
+
+TOPIC_ROOT = "/roboy/simulation/"
+COLLISION_TOPIC = TOPIC_ROOT + "collision"
+BODY_INFO_SRV = TOPIC_ROOT + "body_information"
+
+CONF_PATH = "/src/conf.yml"
+
+class CageInteraction:
+    '''
+    This Class manages all the interactions between the Roboy simulation and the cage 
+    '''
+    def __init__(self, roboy_body_id):
+        rp = RosPack()
+        conf_path = rp.get_path('bulletroboy') + CONF_PATH
+        with open(conf_path) as file:
+            self.parent_link_map = yaml.load(file, Loader=yaml.FullLoader)['parent_link_map']
+        self.body_id = roboy_body_id
+        self.links = []
+        self.initialized = 0
+        self.collision_publisher = rospy.Publisher(COLLISION_TOPIC, Collision, queue_size=1)
+        self.update_body_information = rospy.ServiceProxy(BODY_INFO_SRV, UpdateBodyInformation)
+
+    def initialize(self):
+        """Initializes urdf info and requests a body information update.
+        Args:
+            -
+        Returns:
+            -
+        """
+        self.init_urdf_info()
+        self.call_update_body_information_srv()
+        self.initialized = 1
+
+    def call_update_body_information_srv(self):
+        """Requests a body information update.
+        Args:
+            -
+        Returns:
+            -
+        """
+        try:
+            rospy.loginfo("Wait fo body info service...")
+            rospy.wait_for_service(BODY_INFO_SRV)  
+            rospy.loginfo("Request body information update...")
+
+            resp = self.update_body_information(self.get_body_information())            
+            rospy.loginfo("Got Response...")
+            if resp.ret_code != 0:
+                rospy.logerr("UpdateBodyInformation service failed with messgae: {}.".format(resp.msg))
+
+        except Exception as e:
+            rospy.logerr(e)
+
+    def init_urdf_info(self):
+        """Gets links, free joints, endeffectors and initial link poses in roboy's body.
+        Args:
+            -
+        Returns:
+            -
+        """
+        link = {}
+        link['name'] = 'torso'
+        link['dims'] = self.get_link_bb_dim(-1)
+        link['id'] = -1 
+        link['dims'] = self.get_link_bb_dim(-1)
+        link['init_pose'] = p.getBasePositionAndOrientation(self.body_id)
+        parent_name = self.parent_link_map.get(link['name'])
+        if not parent_name :
+            rospy.loginfo("Link torso is not mapped to a parent link. Will be mapped to itself.")
+            parent_name = link['name']
+        link['parent_name'] = parent_name
+        self.links.append(link)
+        for i in range(p.getNumJoints(self.body_id)):
+            info = p.getJointInfo(self.body_id,i)
+            link = {}
+            name = str(info[12], 'utf-8')
+            link['name'] = name
+            link['dims'] = self.get_link_bb_dim(i)
+            link['id'] = i
+            link['dims'] = self.get_link_bb_dim(i)
+            link['init_pose'] = p.getLinkState(self.body_id, i)[:2]
+            parent_name = self.parent_link_map.get(name)
+            if not parent_name :
+                rospy.loginfo("Link {} with id {} is not mapped to a parent link. Will be mapped to itself.".format(name, i))
+                parent_name = name
+            link['parent_name'] = parent_name
+            self.links.append(link)
+
+    def get_link_bb_dim(self, link_id):
+        """Gets link bounding box dimensions.
+        Args:
+            link_id (int): Index of the link to search.
+        Returns:
+            3darray[float]: x, y, and z dimensions of the bounding box.
+        """
+        aabb = p.getAABB(self.body_id, link_id)
+        aabb = np.array(aabb)
+        return aabb[1] - aabb[0]
+
+    def get_link_info_from_id(self, link_id):
+        """Returns the item in the links list that contains information about the link with the id given.
+        Args:
+            link_id: id of link
+        Returns:
+            link from links list
+        """
+        link = list(filter(lambda link: link['id'] == link_id, self.links))
+        assert len(link) == 1, "Found result length is not 1 : link_id = {}, result_length = {}".format(link_id, len(link))
+        return link[0]
+
+
+    def get_link_info_from_name(self, link_name):
+        """Returns the item in the links list that contains information about the link with the name given.
+        Args:
+            link_name: name of link
+        Returns:
+            link from links list
+        """
+        link = list(filter(lambda l: l['name'] == link_name, self.links))
+
+        assert len(link) == 1, "Found result length is not 1 : link_name = {}, result_length = {}".format(link_name, len(link))
+        return link[0]
+
+    def publish_collision(self, collision):
+        """Publishes collision as a ROS Message.
+
+        Args:
+            collision (list): item of the contact points list output of pybullet getcontactPoints.
+
+        Returns:
+            -
+
+        """
+        contact_pts = []
+        for pt in collision:
+            if pt[9] > 0:
+                link = self.get_link_info_from_id(pt[3])
+                if link['parent_name'] == link['name']:
+                    link_id = pt[3]            
+                else :
+                    link_id = self.get_link_info_from_name(link['parent_name'])["id"]
+
+                contact_pt = ContactPoint()
+
+                #pt[3] == linkIndexA in PyBullet docu
+                contact_pt.linkid = link_id
+
+                #pt[5] == positionOnA in PyBullet docu
+                pos_in_lf = self.get_vector_in_link_frame(link_id, pt[5])
+                contact_pt.position.x = pos_in_lf[0]
+                contact_pt.position.y = pos_in_lf[1]
+                contact_pt.position.z = pos_in_lf[2]
+
+                #pt[7] == contactNormalOnB in PyBullet docu
+                normal_in_lf = self.get_vector_in_link_frame(link_id, pt[7])
+                contact_pt.contactnormal.x = normal_in_lf[0]
+                contact_pt.contactnormal.y = normal_in_lf[1]
+                contact_pt.contactnormal.z = normal_in_lf[2]
+
+                #pt[8] == contactDistance in PyBullet docu
+                contact_pt.contactdistance = pt[8]
+
+                #pt[9] == normalForce in PyBullet docu
+                contact_pt.normalforce = pt[9]
+
+                contact_pts.append(contact_pt)
+
+        if not contact_pts:
+            return
+
+        msg = Collision()
+        msg.header.stamp = rospy.Time.now()
+        msg.contact_points = contact_pts
+
+        self.collision_publisher.publish(msg)
+
+    def get_vector_in_link_frame(self, link, vector):
+        """Transforms a vector from world's coordinates system to link frame.
+
+        Args:
+            link (int): Link id.
+            vector (array[3]): Vector in world's coordinates system.
+
+        Returns:
+            array[3]: The vector in link frame.
+
+        """
+        if(link == -1):
+            frame_pos, frame_orn = (p.getBasePositionAndOrientation(self.body_id))[:2]
+        else:
+            #[0] == linkWorldPosition in PyBullet docu
+            #[1] == linkWorldOrientation in PyBullet docu
+            frame_pos, frame_orn = (p.getLinkState(self.body_id, link))[:2]
+
+        pos_in_LF, orn_in_LF = p.invertTransform(frame_pos, frame_orn)
+
+        pos_in_LF, orn_in_LF = p.multiplyTransforms(pos_in_LF,orn_in_LF,vector,[0,0,0,1])
+
+        return pos_in_LF
+
+    def get_body_information(self):
+        body_information = BodyInformation(link_information=[])
+        for link in self.links:
+            link_info = LinkInformation()
+            link_info.id = link['id']
+            link_info.name = link['name']
+            link_info.dimensions.x = link['dims'][0]
+            link_info.dimensions.y = link['dims'][1]
+            link_info.dimensions.z = link['dims'][2]
+            link_info.init_pose.position.x = link['init_pose'][0][0]
+            link_info.init_pose.position.y = link['init_pose'][0][1]
+            link_info.init_pose.position.z = link['init_pose'][0][2]
+            link_info.init_pose.orientation.x = link['init_pose'][1][0]
+            link_info.init_pose.orientation.y = link['init_pose'][1][1]
+            link_info.init_pose.orientation.z = link['init_pose'][1][2]
+            link_info.init_pose.orientation.w = link['init_pose'][1][3]
+            body_information.link_information.append(link_info)
+        return body_information

src/conf.yml

@@ -0,0 +1,81 @@
+parent_link_map:
+  torso: torso
+  upperarm_right: upperarm_right
+  shoulder_right_link1: upperarm_right
+  shoulder_right_link2: upperarm_right
+  lowerarm_right: lowerarm_right
+  elbow_right: lowerarm_right
+  hand_right: hand_right
+  wrist_right_link1: hand_right
+  wrist_right_link2: hand_right
+  wrist_right_link: hand_right
+  rh_palm: hand_right
+  rh_manipulator: hand_right
+  rh_ffknuckle: hand_right
+  rh_ffproximal: hand_right
+  rh_ffmiddle: hand_right
+  rh_ffdistal: hand_right
+  rh_fftip: hand_right
+  rh_mfknuckle: hand_right
+  rh_mfproximal: hand_right
+  rh_mfmiddle: hand_right
+  rh_mfdistal: hand_right
+  rh_mftip: hand_right
+  rh_rfknuckle: hand_right
+  rh_rfproximal: hand_right
+  rh_rfmiddle: hand_right
+  rh_rfdistal: hand_right
+  rh_rftip: hand_right
+  rh_lfmetacarpal: hand_right
+  rh_lfknuckle: hand_right
+  rh_lfproximal: hand_right
+  rh_lfmiddle: hand_right
+  rh_lfdistal: hand_right
+  rh_lftip: hand_right
+  rh_thbase: hand_right
+  rh_thproximal: hand_right
+  rh_thhub: hand_right
+  rh_thmiddle: hand_right
+  upperarm_left: upperarm_left
+  shoulder_left_link1: upperarm_left
+  shoulder_left_link2: upperarm_left
+  lowerarm_left: lowerarm_left
+  elbow_left: lowerarm_left
+  hand_left: hand_left
+  wrist_left_link1: hand_left
+  wrist_left_link2: hand_left
+  wrist_left_link: hand_left
+  lh_palm: hand_left
+  lh_manipulator: hand_left
+  lh_ffknuckle: hand_left
+  lh_ffproximal: hand_left
+  lh_ffmiddle: hand_left
+  lh_ffdistal: hand_left
+  lh_fftip: hand_left
+  lh_mfknuckle: hand_left
+  lh_mfproximal: hand_left
+  lh_mfmiddle: hand_left
+  lh_mfdistal: hand_left
+  lh_mftip: hand_left
+  lh_rfknuckle: hand_left
+  lh_rfproximal: hand_left
+  lh_rfmiddle: hand_left
+  lh_rfdistal: hand_left
+  lh_rftip: hand_left
+  lh_lfmetacarpal: hand_left
+  lh_lfknuckle: hand_left
+  lh_lfproximal: hand_left
+  lh_lfmiddle: hand_left
+  lh_lfdistal: hand_left
+  lh_lftip: hand_left
+  lh_thbase: hand_left
+  lh_thproximal: hand_left
+  lh_thhub: hand_left
+  lh_thmiddle: hand_left
+  lh_thdistal: hand_left
+  lh_thtip: hand_left
+  head: head
+  head_link2: head
+  head_link1: head
+  camera_left: head
+  camera_right: head