ravens/tasks/defs_cables.py
def add_cable(self, env, size_range, info={}, cable_idx=0,
            direction='z', max_force=100):
        """Add a cable like Andy does it in his cable environment.

        Add each `part_id` to (a) env.objects, (b) object_points, (c) _IDs,
        and (d) cable_bead_IDs. For (b) it is because, like the sweeping env,
        the demonstrator detects the one farthest from the goal to use as the
        pick, and `object_points` is also used to tally up items that are
        within the zone, to compute the reward, or the net % improvement, so
        if we add more cables, all beads in them need to be added to this
        dict. But I'm adding (d) so that we can distinguish between bead vs
        non-bead objects --- just in case!

        When iterating through the number of parts, ensure that the given
        cable is _separate_ from prior cables, in case there are more than
        one. ALL beads are put in the `env.objects` list.

        The zone_range is used because we need the cables to start outside of
        the zone. However, we should check if sampling multiple cables will
        work; there might not be space to sample a zone and two cables.

        Parameters
        ----------
        :size_range: Used to indicate the area of the target, so the beads
            avoid spawning there.
        :info: Stores relevant stuff, such as for ground-truth targets.
        :cable_idx: Only useful if we spawn multiple cables.
        :direction: Usually we want z, for verticality.

        Returns nothing, but could later return the bead IDs if needed.
        """
        num_parts = self.num_parts
        radius = self.radius
        length = self.length
        color = self.colors[cable_idx] + [1]
        color_end = U.COLORS['yellow'] + [1]

        # Add beaded cable.
        distance = length / num_parts
        position, _ = self.random_pose(env, size_range)
        position = np.float32(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):
            if direction == 'x':
                position[0] += distance
                parent_frame = (distance, 0, 0)
            elif direction == 'y':
                position[1] += distance
                parent_frame = (0, distance, 0)
            else:
                position[2] += distance
                parent_frame = (0, 0, distance)

            part_id = p.createMultiBody(0.1, part_shape, part_visual,
                    basePosition=position)
            if i > 0:
                constraint_id = p.createConstraint(
                        parentBodyUniqueId=env.objects[-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=max_force)

            if (i > 0) and (i < num_parts - 1):
                p.changeVisualShape(part_id, -1, rgbaColor=color)
            elif i == num_parts - 1:
                p.changeVisualShape(part_id, -1, rgbaColor=color_end)

            # 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)

            # Get target placing positions for each cable bead, if applicable.
            if self._name == 'cable-shape' or self._name == 'cable-shape-notarget' or \
                    self._name == 'cable-line-notarget':
                # ----------------------------------------------------------- #
                # Here, zone_pose = square_pose, unlike Ravens cable, where the
                # zone_pose is shifted so that its center matches the straight
                # line segment center. For `true_position`, we use `zone_pose`
                # but apply the correct offset to deal with the sides. Note
                # that `length` is the size of a fully smoothed cable, BUT we
                # made a rectangle with each side <= length.
                # ----------------------------------------------------------- #
                lx = info['lengthx']
                ly = info['lengthy']
                r = radius

                if info['nb_sides'] == 1:
                    # Here it's just a straight line on the 'lx' side.
                    x_coord = lx / 2 - (distance * i)
                    y_coord = 0
                    true_position = (x_coord - r, y_coord, 0)

                elif info['nb_sides'] == 2:
                    # Start from lx side, go 'left' to the pivot point, then on
                    # the ly side, go 'upwards' but offset by `i`. For radius
                    # offset, I just got this by tuning. XD
                    if i < info['cutoff']:
                        x_coord = lx / 2 - (distance * i)
                        y_coord = -ly / 2
                        true_position = (x_coord - r, y_coord, 0)
                    else:
                        x_coord = -lx / 2
                        y_coord = -ly / 2 + (distance * (i - info['cutoff']))
                        true_position = (x_coord, y_coord + r, 0)

                elif info['nb_sides'] == 3:
                    # Start from positive lx, positive ly, go down to first
                    # pivot. Then go left to the second pivot, then up again.
                    # For v1, division by two is because we assume BOTH of the
                    # 'ly edges' were divided by two.
                    v1 = (self.num_parts - info['cutoff']) / 2
                    v2 = self.num_parts - v1
                    if i < v1:
                        x_coord = lx / 2
                        y_coord = ly / 2 - (distance * i)
                        true_position = (x_coord, y_coord - r, 0)
                    elif i < v2:
                        x_coord = lx / 2 - (distance * (i - v1))
                        y_coord = -ly / 2
                        true_position = (x_coord - r, y_coord, 0)
                    else:
                        x_coord = -lx / 2
                        y_coord = -ly / 2 + (distance * (i - v2))
                        true_position = (x_coord, y_coord + r, 0)

                elif info['nb_sides'] == 4:
                    # I think this is similar to the 2-side case: we start in
                    # the same direction and go counter-clockwise.
                    v1 = info['cutoff'] / 2
                    v2 = num_parts / 2
                    v3 = (num_parts + info['cutoff']) / 2
                    if i < v1:
                        x_coord = lx / 2 - (distance * i)
                        y_coord = -ly / 2
                        true_position = (x_coord, y_coord, 0)
                    elif i < v2:
                        x_coord = -lx / 2
                        y_coord = -ly / 2 + (distance * (i - v1))
                        true_position = (x_coord, y_coord, 0)
                    elif i < v3:
                        x_coord = -lx / 2 + (distance * (i - v2))
                        y_coord = ly / 2
                        true_position = (x_coord, y_coord, 0)
                    else:
                        x_coord = lx / 2
                        y_coord = ly / 2 - (distance * (i - v3))
                        true_position = (x_coord, y_coord, 0)

                # Map true_position onto the workspace from zone_pose.
                true_position = self.apply(self.zone_pose, true_position)

                # See `cable.py`: just get the places and steps set.
                self.goal['places'][part_id] = (true_position, (0, 0, 0, 1.))
                symmetry = 0
                self.goal['steps'][0][part_id] = (symmetry, [part_id])

                # Debugging target zones.
                if self.target_debug_markers:
                    sq_pose = ((true_position[0], true_position[1], 0.002), (0,0,0,1))
                    sq_template = 'assets/square/square-template-allsides-blue.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 like Andy does it in his cable environment.

        Add each `part_id` to (a) env.objects, (b) object_points, (c) _IDs,
        and (d) cable_bead_IDs. For (b) it is because, like the sweeping env,
        the demonstrator detects the one farthest from the goal to use as the
        pick, and `object_points` is also used to tally up items that are
        within the zone, to compute the reward, or the net % improvement, so
        if we add more cables, all beads in them need to be added to this
        dict. But I'm adding (d) so that we can distinguish between bead vs
        non-bead objects --- just in case!

        When iterating through the number of parts, ensure that the given
        cable is _separate_ from prior cables, in case there are more than
        one. ALL beads are put in the `env.objects` list.

        The zone_range is used because we need the cables to start outside of
        the zone. However, we should check if sampling multiple cables will
        work; there might not be space to sample a zone and two cables.

        Parameters
        ----------
        :size_range: Used to indicate the area of the target, so the beads
            avoid spawning there.
        :info: Stores relevant stuff, such as for ground-truth targets.
        :cable_idx: Only useful if we spawn multiple cables.
        :direction: Usually we want z, for verticality.

        Returns nothing, but could later return the bead IDs if needed.
        
Add a cable like Andy does it in his cable environment.

When iterating through the number of parts, ensure that the given
cable is _separate_ from prior cables, in case there are more than
one. ALL beads are put in the `env.objects` list.

The zone_range is used because we need the cables to start outside of
the zone. However, we should check if sampling multiple cables will
work; there might not be space to sample a zone and two cables.

Parameters
Add a cable like Andy does it in his cable environment . When iterating through the number of parts ensure that the given cable is _separate_ from prior cables in case there are more than one . ALL beads are put in the ` env . objects ` list . The zone_range is used because we need the cables to start outside of the zone . However we should check if sampling multiple cables will work ; there might not be space to sample a zone and two cables . Parameters
def add_cable(self, env, size_range, info={}, cable_idx=0,
            direction='z', max_force=100):
        num_parts = self.num_parts
        radius = self.radius
        length = self.length
        color = self.colors[cable_idx] + [1]
        color_end = U.COLORS['yellow'] + [1]
        distance = length / num_parts
        position, _ = self.random_pose(env, size_range)
        position = np.float32(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):
            if direction == 'x':
                position[0] += distance
                parent_frame = (distance, 0, 0)
            elif direction == 'y':
                position[1] += distance
                parent_frame = (0, distance, 0)
            else:
                position[2] += distance
                parent_frame = (0, 0, distance)
            part_id = p.createMultiBody(0.1, part_shape, part_visual,
                    basePosition=position)
            if i > 0:
                constraint_id = p.createConstraint(
                        parentBodyUniqueId=env.objects[-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=max_force)
            if (i > 0) and (i < num_parts - 1):
                p.changeVisualShape(part_id, -1, rgbaColor=color)
            elif i == num_parts - 1:
                p.changeVisualShape(part_id, -1, rgbaColor=color_end)
            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 self._name == 'cable-shape' or self._name == 'cable-shape-notarget' or \
                    self._name == 'cable-line-notarget':
                lx = info['lengthx']
                ly = info['lengthy']
                r = radius
                if info['nb_sides'] == 1:
                    x_coord = lx / 2 - (distance * i)
                    y_coord = 0
                    true_position = (x_coord - r, y_coord, 0)
                elif info['nb_sides'] == 2:
                    if i < info['cutoff']:
                        x_coord = lx / 2 - (distance * i)
                        y_coord = -ly / 2
                        true_position = (x_coord - r, y_coord, 0)
                    else:
                        x_coord = -lx / 2
                        y_coord = -ly / 2 + (distance * (i - info['cutoff']))
                        true_position = (x_coord, y_coord + r, 0)
                elif info['nb_sides'] == 3:
                    v1 = (self.num_parts - info['cutoff']) / 2
                    v2 = self.num_parts - v1
                    if i < v1:
                        x_coord = lx / 2
                        y_coord = ly / 2 - (distance * i)
                        true_position = (x_coord, y_coord - r, 0)
                    elif i < v2:
                        x_coord = lx / 2 - (distance * (i - v1))
                        y_coord = -ly / 2
                        true_position = (x_coord - r, y_coord, 0)
                    else:
                        x_coord = -lx / 2
                        y_coord = -ly / 2 + (distance * (i - v2))
                        true_position = (x_coord, y_coord + r, 0)
                elif info['nb_sides'] == 4:
                    v1 = info['cutoff'] / 2
                    v2 = num_parts / 2
                    v3 = (num_parts + info['cutoff']) / 2
                    if i < v1:
                        x_coord = lx / 2 - (distance * i)
                        y_coord = -ly / 2
                        true_position = (x_coord, y_coord, 0)
                    elif i < v2:
                        x_coord = -lx / 2
                        y_coord = -ly / 2 + (distance * (i - v1))
                        true_position = (x_coord, y_coord, 0)
                    elif i < v3:
                        x_coord = -lx / 2 + (distance * (i - v2))
                        y_coord = ly / 2
                        true_position = (x_coord, y_coord, 0)
                    else:
                        x_coord = lx / 2
                        y_coord = ly / 2 - (distance * (i - v3))
                        true_position = (x_coord, y_coord, 0)
                true_position = self.apply(self.zone_pose, true_position)
                self.goal['places'][part_id] = (true_position, (0, 0, 0, 1.))
                symmetry = 0
                self.goal['steps'][0][part_id] = (symmetry, [part_id])
                if self.target_debug_markers:
                    sq_pose = ((true_position[0], true_position[1], 0.002), (0,0,0,1))
                    sq_template = 'assets/square/square-template-allsides-blue.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 ( self , env , size_range , info = { } , cable_idx = 0 , direction = 'z' , max_force = 100 ) : num_parts = self . num_parts radius = self . radius length = self . length color = self . colors [ cable_idx ] + [ 1 ] color_end = U . COLORS [ 'yellow' ] + [ 1 ] distance = length / num_parts position , _ = self . random_pose ( env , size_range ) position = np . float32 ( 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 ) : if direction == 'x' : position [ 0 ] += distance parent_frame = ( distance , 0 , 0 ) elif direction == 'y' : position [ 1 ] += distance parent_frame = ( 0 , distance , 0 ) else : position [ 2 ] += distance parent_frame = ( 0 , 0 , distance ) part_id = p . createMultiBody ( 0.1 , part_shape , part_visual , basePosition = position ) if i > 0 : constraint_id = p . createConstraint ( parentBodyUniqueId = env . objects [ - 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 = max_force ) if ( i > 0 ) and ( i < num_parts - 1 ) : p . changeVisualShape ( part_id , - 1 , rgbaColor = color ) elif i == num_parts - 1 : p . changeVisualShape ( part_id , - 1 , rgbaColor = color_end ) 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 self . _name == 'cable-shape' or self . _name == 'cable-shape-notarget' or self . _name == 'cable-line-notarget' : lx = info [ 'lengthx' ] ly = info [ 'lengthy' ] r = radius if info [ 'nb_sides' ] == 1 : x_coord = lx / 2 - ( distance * i ) y_coord = 0 true_position = ( x_coord - r , y_coord , 0 ) elif info [ 'nb_sides' ] == 2 : if i < info [ 'cutoff' ] : x_coord = lx / 2 - ( distance * i ) y_coord = - ly / 2 true_position = ( x_coord - r , y_coord , 0 ) else : x_coord = - lx / 2 y_coord = - ly / 2 + ( distance * ( i - info [ 'cutoff' ] ) ) true_position = ( x_coord , y_coord + r , 0 ) elif info [ 'nb_sides' ] == 3 : v1 = ( self . num_parts - info [ 'cutoff' ] ) / 2 v2 = self . num_parts - v1 if i < v1 : x_coord = lx / 2 y_coord = ly / 2 - ( distance * i ) true_position = ( x_coord , y_coord - r , 0 ) elif i < v2 : x_coord = lx / 2 - ( distance * ( i - v1 ) ) y_coord = - ly / 2 true_position = ( x_coord - r , y_coord , 0 ) else : x_coord = - lx / 2 y_coord = - ly / 2 + ( distance * ( i - v2 ) ) true_position = ( x_coord , y_coord + r , 0 ) elif info [ 'nb_sides' ] == 4 : v1 = info [ 'cutoff' ] / 2 v2 = num_parts / 2 v3 = ( num_parts + info [ 'cutoff' ] ) / 2 if i < v1 : x_coord = lx / 2 - ( distance * i ) y_coord = - ly / 2 true_position = ( x_coord , y_coord , 0 ) elif i < v2 : x_coord = - lx / 2 y_coord = - ly / 2 + ( distance * ( i - v1 ) ) true_position = ( x_coord , y_coord , 0 ) elif i < v3 : x_coord = - lx / 2 + ( distance * ( i - v2 ) ) y_coord = ly / 2 true_position = ( x_coord , y_coord , 0 ) else : x_coord = lx / 2 y_coord = ly / 2 - ( distance * ( i - v3 ) ) true_position = ( x_coord , y_coord , 0 ) true_position = self . apply ( self . zone_pose , true_position ) self . goal [ 'places' ] [ part_id ] = ( true_position , ( 0 , 0 , 0 , 1. ) ) symmetry = 0 self . goal [ 'steps' ] [ 0 ] [ part_id ] = ( symmetry , [ part_id ] ) if self . target_debug_markers : sq_pose = ( ( true_position [ 0 ] , true_position [ 1 ] , 0.002 ) , ( 0 , 0 , 0 , 1 ) ) sq_template = 'assets/square/square-template-allsides-blue.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 like Andy does it in his cable environment.
Add a cable like Andy does it in his cable environment .
"""Add a cable like Andy does it in his cable environment.

        Add each `part_id` to (a) env.objects, (b) object_points, (c) _IDs,
        and (d) cable_bead_IDs. For (b) it is because, like the sweeping env,
        the demonstrator detects the one farthest from the goal to use as the
        pick, and `object_points` is also used to tally up items that are
        within the zone, to compute the reward, or the net % improvement, so
        if we add more cables, all beads in them need to be added to this
        dict. But I'm adding (d) so that we can distinguish between bead vs
        non-bead objects --- just in case!

        When iterating through the number of parts, ensure that the given
        cable is _separate_ from prior cables, in case there are more than
        one. ALL beads are put in the `env.objects` list.

        The zone_range is used because we need the cables to start outside of
        the zone. However, we should check if sampling multiple cables will
        work; there might not be space to sample a zone and two cables.

        Parameters
        ----------
        :size_range: Used to indicate the area of the target, so the beads
            avoid spawning there.
        :info: Stores relevant stuff, such as for ground-truth targets.
        :cable_idx: Only useful if we spawn multiple cables.
        :direction: Usually we want z, for verticality.

        Returns nothing, but could later return the bead IDs if needed.
        """ # Add beaded cable. # Iterate through parts and create constraints as needed. # The usual for tracking IDs. Four things to add. # Get target placing positions for each cable bead, if applicable. # ----------------------------------------------------------- # # Here, zone_pose = square_pose, unlike Ravens cable, where the # zone_pose is shifted so that its center matches the straight # line segment center. For `true_position`, we use `zone_pose` # but apply the correct offset to deal with the sides. Note # that `length` is the size of a fully smoothed cable, BUT we # made a rectangle with each side <= length. # ----------------------------------------------------------- # # Here it's just a straight line on the 'lx' side. # Start from lx side, go 'left' to the pivot point, then on # the ly side, go 'upwards' but offset by `i`. For radius # offset, I just got this by tuning. XD # Start from positive lx, positive ly, go down to first # pivot. Then go left to the second pivot, then up again. # For v1, division by two is because we assume BOTH of the # 'ly edges' were divided by two. # I think this is similar to the 2-side case: we start in # the same direction and go counter-clockwise. # Map true_position onto the workspace from zone_pose. # See `cable.py`: just get the places and steps set. # Debugging target zones.
self
env
size_range
info
cable_idx
direction
max_force
size_range
Used to indicate the area of the target, so the beads
avoid spawning there.
Used to indicate the area of the target so the beads avoid spawning there .
info
Stores relevant stuff, such as for ground-truth targets.
Stores relevant stuff such as for ground - truth targets .
cable_idx
Only useful if we spawn multiple cables.
Only useful if we spawn multiple cables .
direction
Usually we want z, for verticality.
Returns nothing, but could later return the bead IDs if needed.
Usually we want z for verticality . Returns nothing but could later return the bead IDs if needed .