ravens/tasks/defs_cables.py
def add_cable_ring(self, env, info={}, cable_idx=0):
        """Add a cable, but make it connected at both ends to form a ring.

        For consistency, add each `part_id` to various information tracking
        lists and dictionaries (see `add_cable` documentation).

        :cable_idx: Used for environments with more than one cable.
        :info: Stores relevant stuff, such as for ground-truth targets.
        """
        def rad_to_deg(rad):
            return (rad * 180.0) / np.pi

        def get_discretized_rotations(num_rotations):
            # counter-clockwise
            theta = i * (2 * np.pi) / num_rotations
            return (theta, rad_to_deg(theta))

        # Bead properties.
        num_parts = self.num_parts
        radius = self.radius
        color = self.colors[cable_idx] + [1]

        # The `ring_radius` (not the bead radius!) has to be tuned somewhat.
        # Try to make sure the beads don't have notable gaps between them.
        ring_radius = info['ring_radius']
        beads = []
        bead_positions_l = []

        # Add beaded cable. Here, `position` is the circle center.
        position = np.float32(info['center_position'])
        part_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[radius]*3)
        part_visual = p.createVisualShape(p.GEOM_SPHERE, radius=radius*1.5)

        # Iterate through parts and create constraints as needed.
        for i in range(num_parts):
            angle_rad, _ = get_discretized_rotations(num_parts)
            px = ring_radius * np.cos(angle_rad)
            py = ring_radius * np.sin(angle_rad)
            #print(f'pos: {px:0.2f}, {py:0.2f}, angle: {angle_rad:0.2f}, {angle_deg:0.1f}')
            bead_position = np.float32([position[0] + px, position[1] + py, 0.01])
            part_id = p.createMultiBody(0.1, part_shape, part_visual,
                    basePosition=bead_position)
            p.changeVisualShape(part_id, -1, rgbaColor=color)

            if i > 0:
                parent_frame = bead_position - bead_positions_l[-1]
                constraint_id = p.createConstraint(
                        parentBodyUniqueId=beads[-1],
                        parentLinkIndex=-1,
                        childBodyUniqueId=part_id,
                        childLinkIndex=-1,
                        jointType=p.JOINT_POINT2POINT,
                        jointAxis=(0, 0, 0),
                        parentFramePosition=parent_frame,
                        childFramePosition=(0, 0, 0))
                p.changeConstraint(constraint_id, maxForce=100)

            # Make a constraint with i=0. Careful with `parent_frame`!
            if i == num_parts - 1:
                parent_frame = bead_positions_l[0] - bead_position
                constraint_id = p.createConstraint(
                        parentBodyUniqueId=part_id,
                        parentLinkIndex=-1,
                        childBodyUniqueId=beads[0],
                        childLinkIndex=-1,
                        jointType=p.JOINT_POINT2POINT,
                        jointAxis=(0, 0, 0),
                        parentFramePosition=parent_frame,
                        childFramePosition=(0, 0, 0))
                p.changeConstraint(constraint_id, maxForce=100)

            # Track beads.
            beads.append(part_id)
            bead_positions_l.append(bead_position)

            # The usual for tracking IDs. Four things to add.
            self.cable_bead_IDs.append(part_id)
            self._IDs[part_id] = f'cable_part_{str(part_id).zfill(2)}'
            env.objects.append(part_id)
            self.object_points[part_id] = np.float32((0, 0, 0)).reshape(3, 1)

            if 'cable-ring' in self._name:
                # For targets, we can assume the starting position gives us
                # that. Later, apply a random force on the ring to perturb it.
                true_position = (bead_position[0], bead_position[1], 0)
                self.goal['places'][part_id] = (true_position, (0, 0, 0, 1.))
                symmetry = 0
                self.goal['steps'][0][part_id] = (symmetry, [part_id])

                # Make the true positions visible if desired.
                if info['targets_visible']:
                    sq_pose = ((true_position[0], true_position[1], 0.002), (0,0,0,1))
                    sq_template = 'assets/square/square-template-allsides-green.urdf'
                    replace = {'DIM': (0.003,), 'HALF': (0.003 / 2,)}
                    urdf = self.fill_template(sq_template, replace)
                    env.add_object(urdf, sq_pose, fixed=True)
                    os.remove(urdf)
            else:
                print(f'Warning, env {self._name} will not have goals.')
Add a cable, but make it connected at both ends to form a ring.

        For consistency, add each `part_id` to various information tracking
        lists and dictionaries (see `add_cable` documentation).

        :cable_idx: Used for environments with more than one cable.
        :info: Stores relevant stuff, such as for ground-truth targets.
        
Add a cable, but make it connected at both ends to form a ring.
For consistency, add each `part_id` to various information tracking
lists and dictionaries .
Add a cable but make it connected at both ends to form a ring . For consistency add each ` part_id ` to various information tracking lists and dictionaries .
def add_cable_ring(self, env, info={}, cable_idx=0):
        def rad_to_deg(rad):
            return (rad * 180.0) / np.pi
        def get_discretized_rotations(num_rotations):
            theta = i * (2 * np.pi) / num_rotations
            return (theta, rad_to_deg(theta))
        num_parts = self.num_parts
        radius = self.radius
        color = self.colors[cable_idx] + [1]
        ring_radius = info['ring_radius']
        beads = []
        bead_positions_l = []
        position = np.float32(info['center_position'])
        part_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[radius]*3)
        part_visual = p.createVisualShape(p.GEOM_SPHERE, radius=radius*1.5)
        for i in range(num_parts):
            angle_rad, _ = get_discretized_rotations(num_parts)
            px = ring_radius * np.cos(angle_rad)
            py = ring_radius * np.sin(angle_rad)
            bead_position = np.float32([position[0] + px, position[1] + py, 0.01])
            part_id = p.createMultiBody(0.1, part_shape, part_visual,
                    basePosition=bead_position)
            p.changeVisualShape(part_id, -1, rgbaColor=color)
            if i > 0:
                parent_frame = bead_position - bead_positions_l[-1]
                constraint_id = p.createConstraint(
                        parentBodyUniqueId=beads[-1],
                        parentLinkIndex=-1,
                        childBodyUniqueId=part_id,
                        childLinkIndex=-1,
                        jointType=p.JOINT_POINT2POINT,
                        jointAxis=(0, 0, 0),
                        parentFramePosition=parent_frame,
                        childFramePosition=(0, 0, 0))
                p.changeConstraint(constraint_id, maxForce=100)
            if i == num_parts - 1:
                parent_frame = bead_positions_l[0] - bead_position
                constraint_id = p.createConstraint(
                        parentBodyUniqueId=part_id,
                        parentLinkIndex=-1,
                        childBodyUniqueId=beads[0],
                        childLinkIndex=-1,
                        jointType=p.JOINT_POINT2POINT,
                        jointAxis=(0, 0, 0),
                        parentFramePosition=parent_frame,
                        childFramePosition=(0, 0, 0))
                p.changeConstraint(constraint_id, maxForce=100)
            beads.append(part_id)
            bead_positions_l.append(bead_position)
            self.cable_bead_IDs.append(part_id)
            self._IDs[part_id] = f'cable_part_{str(part_id).zfill(2)}'
            env.objects.append(part_id)
            self.object_points[part_id] = np.float32((0, 0, 0)).reshape(3, 1)
            if 'cable-ring' in self._name:
                true_position = (bead_position[0], bead_position[1], 0)
                self.goal['places'][part_id] = (true_position, (0, 0, 0, 1.))
                symmetry = 0
                self.goal['steps'][0][part_id] = (symmetry, [part_id])
                if info['targets_visible']:
                    sq_pose = ((true_position[0], true_position[1], 0.002), (0,0,0,1))
                    sq_template = 'assets/square/square-template-allsides-green.urdf'
                    replace = {'DIM': (0.003,), 'HALF': (0.003 / 2,)}
                    urdf = self.fill_template(sq_template, replace)
                    env.add_object(urdf, sq_pose, fixed=True)
                    os.remove(urdf)
            else:
                print(f'Warning, env {self._name} will not have goals.')
def add_cable_ring ( self , env , info = { } , cable_idx = 0 ) : def rad_to_deg ( rad ) : return ( rad * 180.0 ) / np . pi def get_discretized_rotations ( num_rotations ) : theta = i * ( 2 * np . pi ) / num_rotations return ( theta , rad_to_deg ( theta ) ) num_parts = self . num_parts radius = self . radius color = self . colors [ cable_idx ] + [ 1 ] ring_radius = info [ 'ring_radius' ] beads = [ ] bead_positions_l = [ ] position = np . float32 ( info [ 'center_position' ] ) part_shape = p . createCollisionShape ( p . GEOM_BOX , halfExtents = [ radius ] * 3 ) part_visual = p . createVisualShape ( p . GEOM_SPHERE , radius = radius * 1.5 ) for i in range ( num_parts ) : angle_rad , _ = get_discretized_rotations ( num_parts ) px = ring_radius * np . cos ( angle_rad ) py = ring_radius * np . sin ( angle_rad ) bead_position = np . float32 ( [ position [ 0 ] + px , position [ 1 ] + py , 0.01 ] ) part_id = p . createMultiBody ( 0.1 , part_shape , part_visual , basePosition = bead_position ) p . changeVisualShape ( part_id , - 1 , rgbaColor = color ) if i > 0 : parent_frame = bead_position - bead_positions_l [ - 1 ] constraint_id = p . createConstraint ( parentBodyUniqueId = beads [ - 1 ] , parentLinkIndex = - 1 , childBodyUniqueId = part_id , childLinkIndex = - 1 , jointType = p . JOINT_POINT2POINT , jointAxis = ( 0 , 0 , 0 ) , parentFramePosition = parent_frame , childFramePosition = ( 0 , 0 , 0 ) ) p . changeConstraint ( constraint_id , maxForce = 100 ) if i == num_parts - 1 : parent_frame = bead_positions_l [ 0 ] - bead_position constraint_id = p . createConstraint ( parentBodyUniqueId = part_id , parentLinkIndex = - 1 , childBodyUniqueId = beads [ 0 ] , childLinkIndex = - 1 , jointType = p . JOINT_POINT2POINT , jointAxis = ( 0 , 0 , 0 ) , parentFramePosition = parent_frame , childFramePosition = ( 0 , 0 , 0 ) ) p . changeConstraint ( constraint_id , maxForce = 100 ) beads . append ( part_id ) bead_positions_l . append ( bead_position ) self . cable_bead_IDs . append ( part_id ) self . _IDs [ part_id ] = f'cable_part_{str(part_id).zfill(2)}' env . objects . append ( part_id ) self . object_points [ part_id ] = np . float32 ( ( 0 , 0 , 0 ) ) . reshape ( 3 , 1 ) if 'cable-ring' in self . _name : true_position = ( bead_position [ 0 ] , bead_position [ 1 ] , 0 ) self . goal [ 'places' ] [ part_id ] = ( true_position , ( 0 , 0 , 0 , 1. ) ) symmetry = 0 self . goal [ 'steps' ] [ 0 ] [ part_id ] = ( symmetry , [ part_id ] ) if info [ 'targets_visible' ] : sq_pose = ( ( true_position [ 0 ] , true_position [ 1 ] , 0.002 ) , ( 0 , 0 , 0 , 1 ) ) sq_template = 'assets/square/square-template-allsides-green.urdf' replace = { 'DIM' : ( 0.003 , ) , 'HALF' : ( 0.003 / 2 , ) } urdf = self . fill_template ( sq_template , replace ) env . add_object ( urdf , sq_pose , fixed = True ) os . remove ( urdf ) else : print ( f'Warning, env {self._name} will not have goals.' )
Add a cable, but make it connected at both ends to form a ring.
Add a cable but make it connected at both ends to form a ring .
"""Add a cable, but make it connected at both ends to form a ring.

        For consistency, add each `part_id` to various information tracking
        lists and dictionaries (see `add_cable` documentation).

        :cable_idx: Used for environments with more than one cable.
        :info: Stores relevant stuff, such as for ground-truth targets.
        """ # counter-clockwise # Bead properties. # The `ring_radius` (not the bead radius!) has to be tuned somewhat. # Try to make sure the beads don't have notable gaps between them. # Add beaded cable. Here, `position` is the circle center. # Iterate through parts and create constraints as needed. #print(f'pos: {px:0.2f}, {py:0.2f}, angle: {angle_rad:0.2f}, {angle_deg:0.1f}') # Make a constraint with i=0. Careful with `parent_frame`! # Track beads. # The usual for tracking IDs. Four things to add. # For targets, we can assume the starting position gives us # that. Later, apply a random force on the ring to perturb it. # Make the true positions visible if desired.
self
env
info
cable_idx
cable_idx
Used for environments with more than one cable.
Used for environments with more than one cable .
info
Stores relevant stuff, such as for ground-truth targets.
Stores relevant stuff such as for ground - truth targets .