ravens/environment.py
def movej(self, targj, speed=0.01, t_lim=20):
        """Move UR5 to target joint configuration."""
        t0 = time.time()
        while (time.time() - t0) < t_lim:
            currj = [p.getJointState(self.ur5, i)[0] for i in self.joints]
            currj = np.array(currj)
            diffj = targj - currj
            if all(np.abs(diffj) < 1e-2):
                return True

            # Move with constant velocity
            norm = np.linalg.norm(diffj)
            v = diffj / norm if norm > 0 else 0
            stepj = currj + v * speed
            gains = np.ones(len(self.joints))
            p.setJointMotorControlArray(
                bodyIndex=self.ur5,
                jointIndices=self.joints,
                controlMode=p.POSITION_CONTROL,
                targetPositions=stepj,
                positionGains=gains)
            time.sleep(0.001)
        print('Warning: movej exceeded {} sec timeout. Skipping.'.format(t_lim))
        return False
Move UR5 to target joint configuration.
Move UR5 to target joint configuration.
Move UR5 to target joint configuration .
def movej(self, targj, speed=0.01, t_lim=20):
        t0 = time.time()
        while (time.time() - t0) < t_lim:
            currj = [p.getJointState(self.ur5, i)[0] for i in self.joints]
            currj = np.array(currj)
            diffj = targj - currj
            if all(np.abs(diffj) < 1e-2):
                return True
            norm = np.linalg.norm(diffj)
            v = diffj / norm if norm > 0 else 0
            stepj = currj + v * speed
            gains = np.ones(len(self.joints))
            p.setJointMotorControlArray(
                bodyIndex=self.ur5,
                jointIndices=self.joints,
                controlMode=p.POSITION_CONTROL,
                targetPositions=stepj,
                positionGains=gains)
            time.sleep(0.001)
        print('Warning: movej exceeded {} sec timeout. Skipping.'.format(t_lim))
        return False
def movej ( self , targj , speed = 0.01 , t_lim = 20 ) : t0 = time . time ( ) while ( time . time ( ) - t0 ) < t_lim : currj = [ p . getJointState ( self . ur5 , i ) [ 0 ] for i in self . joints ] currj = np . array ( currj ) diffj = targj - currj if all ( np . abs ( diffj ) < 1e-2 ) : return True norm = np . linalg . norm ( diffj ) v = diffj / norm if norm > 0 else 0 stepj = currj + v * speed gains = np . ones ( len ( self . joints ) ) p . setJointMotorControlArray ( bodyIndex = self . ur5 , jointIndices = self . joints , controlMode = p . POSITION_CONTROL , targetPositions = stepj , positionGains = gains ) time . sleep ( 0.001 ) print ( 'Warning: movej exceeded {} sec timeout. Skipping.' . format ( t_lim ) ) return False
Move UR5 to target joint configuration.
Move UR5 to target joint configuration .
"""Move UR5 to target joint configuration.""" # Move with constant velocity
self
targj
speed
t_lim